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Motion of a Rigid Body 



This Chapter shows the fundamental differences between the motion proper- 
ties of a point mass and of a rigid body. The rigid body does not live in a 
Euclidean space. Hence, this Chapter explains which relevant concepts from 
non-Euclidean (i.e., differential) geometry are needed to describe the kinemat- 
ics and dynamics of a rigid body. 



1.1 Introduction 



The motion of a particle mass is easy to describe because its configuration can be 
associated to a point of the three-dimensional Euclidean space. After having chosen 
coordinates, each point can be associated to a triple of real numbers in K 3 ; but the 
most important thing is that the algebraic and topological properties of M 3 correspond 
to real physical properties of the motion of the particle: forces can be added; velocity 
vectors too; magnitudes of vectors correspond to magnitudes of forces and velocities; 
orthogonality of a force and velocity vector gives zero power; the velocity and accelera- 
tion vectors are the time derivatives of the point's position vector; Newton's Laws link a 
three-dimensional force vector to a three-dimensional acceleration vector, through the 
scalar quantity "mass," for point masses, as well as spherically-symmetric rigid bodies 
such as planets and canon balls. 

In contrast to the simplicity of the point mass motion properties, the motion and the 
dynamics of a rigid body are much more complex. A rigid body is composed of an infinite 
number of point masses, which are constrained not to move with respect to each other. 
It turns out that the dimension of the space necessary to describe the configuration 
of a rigid body is six: three dimensions for orientation, and three for translation. And 
the force-acceleration relation is now a full six-by-six matrix, and not a scalar anymore. 
Moreover, the acceleration involved in this dynamic relation is not just the second-order 
time derivative of the position/orientation vector of the rigid body. 

Even the short overview above should make it clear that it is wrong to treat the six 
position/orientation coordinates of a rigid body in the same way as one treats the three 
position coordinates of a point: the geometrical properties of rigid bodies are funda- 
mentally different from the geometrical properties of point masses. For example, if one 
continuously increases one of these six numbers (i.e., one that corresponds to orienta- 
tion representation), the rigid body arrives at the same configuration after every rota- 
tion over 360 degrees; this "curvature" property does not occur when one indefinitely 
increases any of the three coordinates of a point configuration. 

Locally (i.e., in the neighborhood of a specific configuration) it is possible to describe a 
configuration using six real numbers, but this description is not an intrinsic property of 
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the motion. (An intuitive definition of an "intrinsic property" is: any property that does 
not change if one changes the coordinate representation; the total mass of a point or 
body is such an intrinsic property, at least in non-relativistic dynamics.) 

A lot of powerful tools are available which allow to describe the motion of rigid bodies in 
a geometrical and global way. These methods are related to the geometry of lines and 
screws and to the differential geometric concept of a Lie group. This text makes ample 
use of these (and other) mathematical concepts, because their mathematical properties 
correspond perfectly to the (ideal) physical properties of moving rigid bodies. The quality 
of engineering results depends critically on the quality and faithfulness of the mathe- 
matical models on which the engineering is based: especially in robotics, engineers 
have made (and are still making) fundamental errors in their reasoning, for the sole 
reason that that reasoning was based on mathematical properties of the coordinates in 
their model for which no corresponding physical properties exist. 

In a nutshell: it's not because one can use n numbers as coordinates on a given space, 
that the objects in that space have exactly the same properties as the ?i-tuples in E n ! 



1.2 The Euclidean Space 

The space of rigid body motion (position and orientation; translational and angular 
velocity; and translational and angular acceleration) has different properties than the 
well-known three-dimensional Euclidean space we live in. A Euclidean space is a con- 
tinuous set of points together with an extra structure which is able to describe orthog- 
onality and to measure length. This extra structure is the scalar product. We indicate 
an Euclidean space of dimension n with £ (n). The Euclidean space £{n) is therefore the 
set of points of the space. We indicate the three-dimensional space we live in with £(3) 
or simply £ . Sometimes we omit the information about the dimension and just indicate 
the Euclidean space as £. It is important to realize that since a point can be fixed or 
moving with respect to an observer, for a given Euclidean space we need to specify an 
observer which does not move within it. We suppose that there exists an inertial ob- 
server we consider as reference in which the points belonging to the Euclidean space are 
not moving. It is possible to see in (Stramigioli 2001, Stramigioli & Bruyninckx 2001) 
that the hypothesis we use considering a reference Euclidean space hides important 
consequences. 



1.2.1 Points versus vectors 

The points of £ are not vectors but points: they do not have a versus and a direction 
and as such it makes no sense to, for example, sum them! We can associate a vector to 
them only once we have chosen a reference point: if we choose such a reference point 
o g £, we can associate to every point p G £ a vector going from o to p which we indicate 
with (p — o). It is not here the place to go into details, but this association is possible 
since the space is Euclidean: for curved spaces this is not possible. For example, what 
would be the meaning of the "sum" of two points on the surface of the earth? 

However, once we have vectors, we can sum them; for example, it makes sense to 
sum the velocities or accelerations that a moving point mass gets under the influence 
of separate forces acting on the point. And for points, the velocity and acceleration 
are indeed vectors, because they represent the "timed" distance between consecutive 
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Figure 1.1: The distance between points are represented by vectors. 



positions of the same point. These vectors are called free vectors, because their physical 
meaning does not change if one moves them parallel to themselves to another place in 
the space. Mathematically speaking, we use as a vector space the quotient space of all 
different vectors, using parallelism and equal length as equivalence relations. 

For example, as shown in Figure 1.1, we consider the vector (p ~ q) equivalent to the 
vector (to — /) where p, q,l,m e £. The set of these free vectors are denoted with or 
if it is not necessary to show the dimension n. 



1.2.2 The scalar product 



We can now introduce the scalar product which characterizes a Euclidean space. It is a 
map which gives a real number once we give two vectors belonging to as input: 

(,):£* x — ► M ; (v, w) i-> (v, w) . 

The scalar product satisfies the following properties: 

1. (v,w) = (w,v) (symmetry). 

2. (v,v) > (positiveness). 

3. (avi + /3v 2l w) = + f3(v 2 ,w) (linearity). 



1.2.3 Measuring the distance between points 



We can define the distance d(p, q) between the points p and q as the length of (p-q) G £*. 
This distance defines a norm: 

|| -|| ; V h-> y/(v,v). 



1.2.4 Orthogonality 

Orthogonality of vectors v, w ^ Is also defined. We say that two vectors with non-zero 
length are orthogonal if their scalar product is zero: 

V-Lw (v, w) = 0. 

The cosine of an angle is also a consequence of the scalar product. It can be defined 
using only the scalar product: 

(v,w) 

cos v/Lw := 7p-pj — r; — 77. 

\\v\\ ■ \\w\\ 
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1.2.5 Vector Product 

It is also possible to define a vector product (after a choice of orientation (Stramigioli 
2000)!): 

A :£»(3) x£*(3) £*(3); (v, w) ^vAw. 

The vector product is skew-symmetric, i.e., vAw = -~(wAv). The following "tilde-relation" 
is a one-to-one relation between a point and a mapping of points: 

(xi\ I -x 3 x 2 \ 

2=2 I & X = I X3 — Xi . (1-1) 

X3J \—X2 X\ J 

Using this notation, If we choose a coordinate system ^ and express v, w £ £*(3) in \t Q 

as v°, w° £ M 3 , we can write: 

v° Aw° — v°w°, 

where v° is now the matrix corresponding to the above-mentioned mapping. We can 
therefore express the vector product of two vectors as the product of a skew- symmetric 
matrix and a vector. The following equalities hold: 

v° A w° = v°w° = -w° Av° = -w°v° = w T v°. 



Issues about the vector product will be further explained in Remark. 4, p. 14. 



1.2.6 Coordinate Systems 

If we want to work with a Euclidean space, we need to define coordinate systems in 
order to be able to represent points and vectors by means of numbers to work with. 
Formally, we define a coordinate system for an n-dimensional Euclidean space £ (n) 
as an (n + l)-tuple composed of a point o 6 £(n) and n linear independent vectors 
belonging to £*(n): 

* := (o, ei, e 2 , . . . , e„) G £{n) x £»(n) x • 

^ j 

n times 

The symbol capital 'Psi' is used to indicate a coordinate system. 

A special kind of coordinate systems is called orthonormal. Orthonormal coordinate 
systems are defined as those coordinates systems for which 

1 1 ej|| = 1 Vi (unit vectors) , 

and 

(ei,ej)=0 yi^j (orthogonality). 

An orthonormal coordinate system is a system for which its defining vectors are or- 
thogonal with respect to each other, and have all unity length. As an example consider 
Figure 1.2 for the case of £ (2). Part a) represents the set ty representing a coordinate 
system and composed of o, e\ , e 2 ; b) shows it in the way we are used to use it shifting 
the vectors to o; c) shows the parallelogram rule to see how to calculate the coordinates 
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for a point p; and d) shows an orthonormal frame if we suppose that the length of a 
and e 2 is equal to unity. 

We say that an orthonormal coordinate system (o, x, y, z) for £(3) is right handed if, and 
only if, 

x A y — z. 

We implicitly use only right-handed coordinate systems. Given a coordinate system, we 
can associate to each point or vector a set of real numbers representing its coordinates 
in the given frame. The coordinates x t of a point p e £ are: 

x i = (ip - °)> e i) G K, Mi. 
The coordinates of a vector v G f * are: 

.x,; = (?;, ei) G R Vi. 

In 3D we work with £(3) and we indicate sometimes for an orthonormal coordinate 
system x := ei, y := e 2 and I := e3. From now on we will implicitly always use orthogonal, 
positively-oriented coordinate systems. 

Coordinate mapping 

For any Cartesian coordinate system ^ = (oj, Xj, £j), we can define a coordinate map- 
ping V'i which associates to each point a set of numbers in the following way: 

/ ((p-Oi),Xi}\ 

i>i :£(3) ->M 3 ; p ^ ((p-a^ft) . 

\((P- Oi),Zi)J 

We indicate with V'» the coordinate map associated to the coordinate frame ty l = (o l ,x l ,y i , Zi). 
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Change of coordinates 

If we take two coordinate systems, we can consider the mapping representing the 
change of coordinates from one system to the other. A change of coordinates from p 1 
(coordinates with respect to ^i) to p 2 (coordinates with respect to vf^) can be expressed 
as: 

p 2 = (iP 2 o^ 1 )(p 1 ). 

Or, in mapping form: 

R 3 ^£(3)^R 3 ; p^p^p 2 . 



Projective coordinates 



A lot of concepts of importance in kinematics and dynamics are better explained in 
projective terms. It is not here the place to treat projective geometry in details, but we 
will just say a few things in order to use it as a tool. For more mathematical details 
the reader can consult Appendix A. First of all, the set PR™ denotes an ?i-dimensional 
projective real space. Each of the elements of PR™ is an (n + l)-tuple of real numbers. 
Two of these (n + 1) vectors of real numbers a and b in PR™ are related by the projective 
equivalence relation if a is a multiple of b: 

a ~ b 3X ^ : a = Xb. 

It is possible to study three-dimensional Euclidean space using projective coordinates 
PR 3 . A representative of a finite point p G £ can then be: 



P 1 



M 

y 

z 

VJ 



Any point XP 1 with A ^ is also an equivalent projective representation of the same 
point. For reasons which are explained in Appendix A, a point whose numerical rep- 
resentation has the last component equal to zero, represents a point at infinity. Using 
projective geometry, it is therefore possible to consider points at infinity in a consistent 
way. 



1.3 Configurations of a rigid body 

A lot of geometrical and analytical methods have been developed over the last three 
centuries to describe the (changes of) configuration of rigid bodies. The first differential 
geometrical interpretation seems to be due to Study (Selig 1996). In this course we 
will treat the finite twists representation and the Lie group representation of the so- 
called Special Euclidean group 5-5(3). Another possible analytical representation are 
bi-quaternions. Details about this topics can be found in (Selig 1996, Stramigioli 2001, 
Karger & Novak 1978, Abraham & Marsden 1994). The previously mentioned methods 
are all global, geometrical, well-defined methods. This cannot be said of descriptions 
like Euler angles which are only valid locally, and are not as powerful as the methods 
described in the following sections. 
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1.3.1 A Lie Group approach: SE(3) 

Given a certain body in space, we can describe the motion of this body from a certain 
position to a new one. This set of motions are characterized by the fact that they 
are what are called in mathematics orientation preserving isometries. Isometries means 
that the motion does not change the distance between points and orientation preserving 
implies that the motion does not map a right-handed coordinate frame onto a left- 
handed frame. 



Coordinates changes versus motions 

It is a general fact that if we consider a global map of points for an n- dimensional 
Euclidean space £ which is bijective (one-to-one and onto), we can always associate 
to it an equivalent change of coordinates in the following sense. Suppose to have a 
bijective mapping h: 

h:Z->Z s.t. h(jpi) = h(p 2 ) =^ Pi = P2- (1-2) 

Once we have chosen a global 1 coordinate function 

i/j-.E^W 1 , (1.3) 

we can consider the numerical representation of h in the coordinates ip by defining: 

h n :K n ^m. n ; x^(i/.o/iof 1 )(4 (1.4) 

If we then define a new coordinate map 

iP':£^M. n ; p ^ (i/j o h)(p), (1.5) 

the numerical representation h n of h is clearly equivalent to a change of coordinate from 
^ to If, for hypothesis h is a positive isometry, the converse is clearly only true if 
the coordinate functions are isometries from f to R", Coordinate functions satisfying 
this hypothesis are the set of right-handed frames or the set of left-handed frames, but 
not a combination of them. For this reason, we will first study coordinate changes and 
then define motions based on the previous arguments. 



Rotations 

We start by considering changes of coordinates in 2D. Consider two coordinate systems, 
with their origin in common, but rotated over an angle 6 with respect to one another, as 
shown in Figure 1.3. What is the relation between the coordinates of any point p e £(2) 
in *i and The definitions above yield the following for the coordinates of the point 
p in the first frame: 

or, equivalently, 

(jp — o) =x 1 x 1 +yiy 1 , (1.6) 



*A global coordinate function exists since the space S is Euclidean. 
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x x 
Figure 1.3: Change of coordinates under rotation. 

with £1,2/1 G ffi (scalar coordinates) and G 5* (2) (unit position vectors in 2D). 

Similarly, the coordinates p 2 G R 2 of the point p with respect to the second frame are: 

2 ._ fx 2 \ _ f ((p-0),X 2 )\ 

P ■ \y 2 ) {((p- ),y 2 ))> 
The expression for (p — o) in Eq. (1.6), gives: 

2 _ f (^1*1 +2/iyi,^2A = fxi{xi,x 2 ) + yi{yi,x 2 )\ 
\ixiX! + yiyi,y 2 )J \a:i (£1,2/2) + 2/1(2/1,2/2)/ ' 

In matrix form, this gives: 

_ f(xi,x 2 ) (yi,x 2 )\ fx A 
\y 2 ) \(xi,y 2 ) (2/1,2/2)/ \yij ' 

or 

p 2 = i? 2 p 1 , 

where i?f is the rotation matrix, defined as: 

r2 ._ f{x\,x 2 ) (yi,x 2 )\ _( cos (9 sin0\ 
\(&i,2/2) (2/1=2/2)/ \-sm9 cos9J' 

Remark 1 (Orthonormal matrices) The matrix R\ representing the change of coordi- 
nates between two coordinate systems rotated with respect to one another, has the fol- 
lowing properties: 

• det(i? 2 )=J; 

. R\ = (i? 2 )- 1 = (R 2 ) T ; 

• the columns and rows vectors of R\ have length I and are orthogonal. 

These mathematical properties correspond to physical properties of rotations: a rotation 
doesn't change the volume of a set of three independent vectors; the inverse of a rotation 
consists of reversing the projection axes; and, the projections of a set of three orthogonal 
unit vectors give rise to an orthogonal matrix. 

In 3D, if the coordinates we use are all right handed, we have a similar result as in 2D 
for a general rotation around the origin: 

x 2 \ /(xi,x 2 ) (yi,x 2 ) (h,x 2 )\ (xA 

y 2 J = (#1,2/2) (2)1,2/2) (21,2/2) yi J • (1.7) 
22/ \{xi,Z2) (2/1,22) (21,22)/ \ZlJ 

i? 2 
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Rl is therefore almost identical to the two-dimensional case, but now we are dealing 
with a 3 x 3 matrix. 

Remark 2 It is important to note that: 

• The columns of R\ are the unit vectors along the axes of , expressed in the coordi- 
nate system 

• The rows of Rl are the the unit vectors along the axes of expressed in the coor- 
dinate system 

A direct consequence of this is that the inverse of Rl, i.e., R\, is equal to the transpose 
of R\ , This property holds for any rotation matrix R: 

R- 1 = R T 

The following identities are useful properties when rotating any vectors v, w G K 3 : 

R(v Aw) = (Rv) A(Rw), (1.8) 

and 

RwR T = {Rw), (1.9) 
where the "tilde" operator is defined in Eq. (1.1). 

As an example of a rotation in £(3), consider a change of coordinates due to a rotation 
of 6 around y\. The matrix representing this change of coordinates looks like: 

/ cos(0) sin(0)\ 
R\ = 1 . 
\- sin(0) cos(6») / 

It is possible to see that the set of matrices satisfying R~ x = R T is a three-dimensional 
matrix Lie group, which is called the orthonormal group and indicated with 0(3). First, 
it fulfills all properties of a group: 

• Associativity: i?i,i? 2 ,i? 3 G 0(3) =s> (i?ii? 2 )i? 3 = i?i(i? 2 i? 3 )- 

• Identity: / e 0(3). 

• Inverse: R e 0(3) => R- 1 6 0(3), RR- 1 = I. 

Moreover, it is a Lie group, because the composition operation is continuous. Since 
R T R = f Vi? e 0(3), and since the determinant of a product is the product of the 
determinants, the determinant of any matrix in 0(3) must be ±1. This shows that 0(3) 
is composed of two disjoint components, one whose matrices have determinant equal to 
— 1, and one consisting of the matrices with determinant equal to +1. The former is not 
a group by itself, because the product of two such matrices has determinant +1. The 
latter is again a Lie group, called the special orthonormal group, and denoted by SO(3): 

50(3) = {R G K 3x3 ; iT 1 = R T , dct R = 1}. 

Remark 3 Note that the previous rotations are all around a specific point in space, namely 
the common origin of the coordinate systems. This implies that 50(3) can only be used 
to describe rotations around a specific point and not any rotation! This is not explicitly 
mentioned in most of the robotics literature. 
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The Lie group 50(3) is the appropriate mathematical model of the orientations of a rigid 
body. Also the angular velocities of rigid bodies must get a good mathematical model. 
This is given by the so-called Lie algebra of 5*0(3), which is denoted with so (3). A Lie 
algebra describes the local ("first order") properties of its parent Lie group (irrespective 
of what the Lie group represent), while maintaining the geometric properties. 

From the theory on left and right invariant vector fields, Eq. (B.5) and Eq. (B.6), we can 
see that elements of the algebra should look like T L = R^R, or like T R = RR^ 1 , for any 
curve R(t) e SO(3). Because R^ 1 = R T , differentiating the equation R T R = I yields: 

R T R + R T R = => R T R = -(R T R) T , 

which means that the matrix T L is skew-symmetric. Similarly, T R is also skew- symmetric, 
so that so(3) is the vector space of skew- symmetric matrices. Note that due to the struc- 
ture of the matrix commutator of the algebra, the commutation of skew- symmetric ma- 
trices is still skew- symmetric. 

There is a bijective relation between 3x3 skew-symmetric matrices and 3 vectors as it 
was shown in Eq. (1.1). 

It is straightforward to see that the algebra matrix commutator of so(3) corresponds to 
the vector product of the corresponding three vectors: 

[H) X , Uly] = UJ X A UJy WW X , U)y G R 3 . 

The elements of so (3) correspond to the angular velocity of the frames whose relative 
change of coordinates is represented by the matrix R\ G £0(3). We will analyse in more 
detail the general case, i.e., angular and translational velocities. A lot of coordinate rep- 
resentations of rotations exists, besides the above-mentioned skew- symmetric matrix 
representation of 5*0(3); for example, the unit quaternions, which are isomorphic to an- 
other Lie group, namely SU{2) (the Special Unitary group. They are both double covers 
of 50(3) (i.e., each rotation in 50(3) corresponds to two elements in the double cover 
group). In contrast to 50(3), they are both topologically speaking simply connected. 
50(3) is instead not contractible and it is a typical example of a non simply connected 
manifold "without holes." This means that if we define a continuous closed curve in 
50(3) (a ring), there is not always a way to continuously make this ring smaller and 
smaller in order to let it become a point. 

Remark 4 (Vector product) It is a misconception to interpret the vector product in three 
space as an extra structure on £* (the scalar product being the other structure). This 
is NOT the case, because the vector product is a consequence of the fact that for £ the 
Lie group of rotations 50(3) can be intrinsically defined. As we have just seen, the Lie 
algebra so (3) has a commutator defined on it and therefore, after a choice of orientation 
of space, the intrinsic bijection between so (3) and £„ carries the commutation operation 
defined in so (3) over to the vector product in £«. 

It is possible to find two bijections in the following way. A vector of £« is characterized by 
a direction d, an orientation v and a module m. An element q/\so(3) represents an angular 
velocity. It is possible to see that any angular motion leaves a line d invariant. This is 
the first step in this bijection: the vector of £* which we associate to a vector in so(3) will 
have as direction the line left invariant by this rotation (d = d). We still need a module 
and an orientation. It is possible to show that there exists a positive definite metric on 
so(3), called the Killing form (Selig 1996), which defines the magnitude of a rotation in a 
unique, coordinate -independent way. We can therefore choose as m the module of this 
angular velocity vector calculated using the metric on so(3). The only choice we are left 
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Figure 1.4: A general change of coordinates. 



with is that of the orientation. We clearly have two possible choices to orient the line. If 
we look at the rotation motion around its axis as a clockwise motion, we can orient the 
line as going away from us or as coming toward us. In the first case we say that we 
choose a right-handed orientation and in the second case a left-handed orientation. This 
is clearly related to the orientation chosen for the Euclidean space because if we consider 
a point x moving due to the described rotation, we can consider a vector x{t) — o and a 
vector x(t + dt) — o where o is the orthogonal projection 2 of x on the axis of rotation. The 
direction of the vector (x(t) — o) A (x(t + dt) - o) is the associated direction of rotation. 

Notice that if we look at this motion through a mirror, the orientation is changed because, 
under the same rotational motion, a line oriented toward the mirror will be seen through 
the mirror as a line oriented in the opposite direction. This is called a reflection. 



Translations 



Translations are much simpler than rotations. If we have two coordinate systems dis- 
placed with respect to each other, the change of coordinates is simple: 

p i = (p i oHfJ 1 )(p i )=p i +p), (1.10) 

where p* represents the position of the origin of tyj with respect to the origin of and 
expressed as a numerical vector in one of the two frames (either one, because they are 
not rotated with respect to each other) . Obviously this set of translations is also a Lie 
group, due to the well-known additivity of translation vectors. 



General changes of coordinates 



It is now possible to consider a general transformation including any rotation and/or 
translation. Figure 1.4 shows the change of coordinates from a 2D frame vf^ to another 
frame ty 2 - p 2 £ M 3 contains the coordinates of the vector (p — o 2 ) £ £» expressed in the 
frame Taking the coordinates of the following vector equality 

(p - 02) = (p - oi) + (oi - o 2 ), 

yields 

p 2 = R\p x +p\, 



2 We can consider an orthogonal projection since we are in a Euclidean space 
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where p\ are the coordinates of the vector {o\ — o 2 ) expressed in the frame ^> 2 - In matrix 
form, this change of coordinates becomes: 

p 2 \_(R\ p\\(p 



The transformation matrix in this equation is often called the homogeneous (transfor- 
mation) matrix H\ e M 4x4 : 



Hi 



(f 1 



because it represents a coordinate transformation between two frames by a simple ma- 
trix multiplication. This matrix can also be interpreted as a projective coordinate change 
since we can consider the original three-dimensional vector as an element belonging to 



We can recognize a couple of sub-cases, such as a pure translation: 



Hf:= 

and a pure rotation around the origin: 



(# 1 



2 _ / Rl o 3 

or i 



Hf:= 



The homogeneous transformation matrix can represent rotations around a point differ- 
ent from the origin: subsequent coordinate changes compose according to the so-called 
chain rule 

Hf = HfHlHl (1.11) 

for any intermediate frames Hr 2 and So, a rotation around any given point p can 
now be expressed as a translation followed by a rotation and an inverse translation: 



/ p\ (R 0\ (I -p\ (R p- Rp 
1/ I I \0 1 I ~ [ 1 



(1.12) 



The rightmost transformation on the left-hand site transports the coordinates of the 
rigid body that rotates about p from their expression with respect to the origin to their 
expression with respect to the point p\ the middle transformation does the rotation 
about p; and the leftmost transforms all coordinates back to the origin again. It's clear 
that only the point p is mapped on itself for all the possible R, hence the composite 
transformation is a rotation about p. 

The inverse of an homogeneous transformation matrix is simple: 

jji (H 2,-i ({Rl) T -{R\) t pX\_(R\ -R\p\\ 



The set of matrices 



is called the Special Euclidean matrix group, which is of great importance in robotics. 
SE(3) is a Lie group, as proven by the same arguments as for SO(3) above. Hence, 
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o 



X-2 

Figure 1.5: A representation of a motion. 



velocities Hf map from the tangent space T h jSE(3) to se(3) either with left or right 
translations (see Appendix B). The elements of se(3) are of the following form: 

se(3) = { (o o) 5 fi eso(3),t;eM 3 |. (1.13) 

This is proven from the definition by considering H(t) e SE(3) as a parameterized curve, 
and seeing (Appendix B) that elements of the form HE~ X and H~ X H belong to se(3), and 
have the form given in Eq. (1.13). The Lie algebra se(3) has its commutator: 

Furthermore, the algebra commutator of se(3) is such that since 

% = ft i) € «(3) =* [Ti, T 2 ] = (fofA ^ - e «(3). 



Motions versus changes of coordinates 

The relationship between a change of coordinates and a motion follows from Figure 1.5: 

the moved points in the moved coordinate system ^ have the same coordi- 
nates as the original points in the original coordinate system . 

In other words: 

ip2 o h o 1 = /, 

where I is the 4x4 identity matrix if fa are projective coordinates. From 

fa o h o ip^ 1 — fa o ipi o fa o h o ip^ 1 = I, 

it follows that 

fa oh o ipi 1 = H\ , 

which implies that: 
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The representation of the motion from "ti to ^ 2 in the coordinate [ipi o h o ipi *) 
is the inverse of the coordinate change from to * 2 (4>2 o ipi 1 )- 

The same motion expressed in ^ is: 

tp2 o h o ip^ 1 = "02 o tpi 1 o ipi oho ip^ 1 o ipi tp^ = HfH^H^ = H\. 

" v ' " v ' 

/ / 

To summarize: 

• A general change of Cartesian coordinates in £(3) from to tyj can be expressed 
with a matrix of the form: 

F HI i) esm > 

where R\ e 50(3) and p{ e M 3 . 

• For a rigid motion 

h : £ — ► £ ; p 1— > g, 
which maps ^ to $j , we have that 

•I I !',!>'. and q j = Hy , 



1.3.2 The finite twist description 

This description is the geometrical one used in Screw Theory (Ball 1900), and it is based 
on Chasles' Theorem. Chasles' Theorem (which will be proved later on using Lie groups) 
says that any motion of a rigid body can be achieved as a rotation around a geometrical 
line / together with a pure translation along I. The line I is called the (screw) axis of the 
motion. For a pure translation, this axis is at infinity. 

It can furthermore be proven that a pure non-trivial rotation and a pure non-trivial 
translation are commutative if and only if they are around and along the same geomet- 
rical line. Therefore, it is not important if we consider first the rotation and then the 
translation, or vice versa. 



The screw axis 

The axis of the motion is a line. The set of finite lines in space can be parameterized 
by a minimum of four scalars; for example by the so-called Denavit-Hartenberg line co- 
ordinates. Many other non-minimum coordinate sets are available for lines, the most 
common of which are the so-called Pliicker coordinates. These also allow directly to de- 
scribe lines at infinity, which describe pure translations. We will use these coordinates 
in what follows. 

We can define a line as the linear locus passing through two points pi and p 2 . Once we 
have chosen a coordinate frame ^j, we can consider their projective coordinates: 



f Xl \ f X2 \ 

yi and Pi= y2 

zi z z 2 

w w 
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We can consider the following six- dimensional real vector which contains the Plucker 
ray coordinates for the line joining p\ and p 2 with respect to frame 'P,: 



/' 



31 



X2 
S2 



ill 

Si 



V2 
S2 



"-2 
S2 



yi 

Zl 



V2 
"-2 



Zl 
•''1 



Z2 
•''2 



Xl 

yi 



X2 

yi 



(1.14) 



defined using 2-by-2 determinants of the coordinates of the points. The question arises 
immediately wether I* is a meaningful numerical representation of the line joining p\ and 
p 2 in the coordinate 5^ ? The answer is yes if we consider it as an element belonging 
to PR 5 instead of as an element belonging to M 6 (as we will explain shortly), and the 
advantage of this representation is that it automatically allows to represent lines at 
infinity without any extra definitions! The previous coordinates are well posed and 
useful for the following reasons: 



• If instead of taking P{ and P^ we take two other equivalent projective coordinates 
ctP{ and we obtain the same projective coordinates for the line, namely af3l l ~ 
l\ 

• If instead of taking P{ we take another point on the line, namely P[ + a(P{ — P|), the 
properties of determinants assure that the line representation does not change. 

• If both points are at infinity (i.e., si = s 2 = 0), V is still properly defined and 
characterized by its first three components being equal to 0. 



It is easily possible to see that Eq. (1. 14) can be written as: 

li= fs2Pl-SiP2\ = fs2Pl-s m \ (115) 
\ PlAj)2 / VP 1 A CP2 -Pl)J 

From this last expression, it is possible to consider two cases: the first one when the 
defining points are finite (si,s 2 ^ 0) and the second when the points are at infinity 
[si, S2 = 0) and therefore describe a line at infinity. In the first case, we have as coordi- 
nates of a finite line a vector of the form: 



r Aw 



(1-16) 



where w is a vector pointing from pi to p 2 and therefore spanning the line, and r can be 
any point on the line since 

r = pi + a(p2 — pi) => r A lo = p\ A u). 
Furthermore, since the representation of the line is projective, for any a^O, 



a 



r Aw 



represents the same line. This finite line can be associated to a rotation axis. If fur- 
thermore, besides the rotation axis we define a direction of the line to consider the 
positive rotation around the axis and a magnitude representing the rotation angle, we 
can completely determine a finite rotation. 

We can give a compact description of a rotation at once if we leave the projective nature 
of the line description and we consider uasa vector with a magnitude and a direction. 
We can then talk about a unit line by considering an u> such that ||w|| = 1 and then call 
a unit line an element of the form: 



w 
r Aw 



(1.17) 
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It is then possible to describe what is called a rotor as a triple |M|) with an oriented 
axis and a magnitude. A pure rotation of 9 around an axis oriented 3 and directed by w 
and passing through r can be expressed with the following rotor: 



r A CJ / ~~ \r /\6Cj) ~ I r A u 



(1.18) 



which should be interpreted as a triple (I, d, 9) where / is a line, d is a chosen positive 
direction and 9 is an associated magnitude. This can also be interpreted as a line vector: 
a vector cj that can move along its spanning line. 

In the second case, for a line at infinity, we obtain a representation of the form 

where u := pi A (p 2 — Pi ) ■ This can also be nicely interpreted as a rotation at infinity which 
turns out to be representative of a pure translation as explained hereafter. Suppose we 
want to consider a rotation around an axis which we want to move toward infinity: ar 
with a — > oo. In order to find a sensible representation, we need to consider a line set 
parameterized by: 



(ar) A (-uj) J \r Ad 



(1.20) 



For any value of a, the moment rAu must be interpreted as the linear motion of a point 
passing through the origin of the coordinate system and rigidly attached to the space 
which is rotating around the axis passing through ar. The larger a, the closer will be 
the linear velocities of the points close to the origin. In the extreme, for a — > oo, all the 
finite points will have a linear motion equal to v := r A u. This implies that a rotation at 
infinity corresponds to a pure translation and it is represented by the following line at 
infinity: 





The pitch 

It is possible to show that there is an intrinsic measure for rotations (the Killing form of 
SO(3)) and therefore it is possible to quantify a rotation around the axis with a number 
9 (with physical, dimensionless units radians). In the same way, being in a Euclidean 
space, we can measure a translation displacement t of a motion intrinsically without 
using any coordinates, but only the metric of the space; its dimensions are, for example, 
in meters. The ratio between the rotation and the translation is called the pitch A of the 
finite motion: 




It's a coordinate-independent property of the motion. 



3 Clearly, implicitly, we have associated a positive direction with a positive rotation; this can be done in two 
ways by considering a right or left convention and corresponds to the orientation of space appearing in the 
term rAu. 
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The Screw 

Geometrically, a screw S is nothing else than a line I together with a scalar pitch A: 

S:= (I, A). 

Since the dimension of the space of lines is four, the dimension of the space of screws 
is five. Note that a screw, as a line, does not have any module information a priori. If 
this module information is given as we did with the line, we get a six-dimensional space 
of measurable screws. 



Chasles' theorem and finite twists 

Chasles' theorem states that any finite rigid body motion can be expressed as a rotation 
around an axis plus a translation along the same axis. We will now prove this theorem 
by first relating the finite motions to Lie groups. One of the properties of Lie groups is 
the possibility to relate vectors belonging to the Lie algebra of a Lie group to elements 
of the Lie group (see Appendix B). For our goal, it is sufficient to realize that for any 
matrix H e SE(3), there exists always at least one 4 matrix f 6 se(3) such that: 

H = e f , 

where the exponential is the usual exponential of matrices, defined as: 

e A = I + A+±A 2 +±A 3 + ... 

This implies that for any motion which can be represented by a matrix H g SE(3) (see 
Sect. 1.3.1), we can consider a new matrix representation that expresses an element of 
se(3) in the form: 

f-(S ;)■ < ij »> 

We can define an equivalent vector representation of T: 




Now suppose for the moment that to ^ 0. In this case, it is easy to see that we can 
always find a vector r and a scalar A such that: 




(1.22) 



where Xuj is the v component along uj and r A uj is the one orthogonal to u. 

It is also easy to find an expression for r and h in the following way. The second part of 
the Chasles decomposition can be written as: 

v = r Auj + Xlu, (1.23) 

and therefore, taking the vector product with uj we obtain: 

wAn = wA(rAw). (1.24) 
4 In certain cases there could be two solutions. 
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With some straightforward geometric considerations, it is furthermore possible to see 
that lo A (r A uj) — r||w||, and therefore we find an expression for r: 

r = ^J. (1.25) 

IMP 

The calculation of the pitch A is even simpler. By taking the scalar product of tu with 
Eq. (1.23) we obtain: 

oj t v = oj T (r Aoj) + \oj t uj, (1.26) 

and since the first term on the right side is zero (from the property of the mixed product), 
we obtain: 

A = ^. (1.27) 

IMP 

Equivalently to Eq. (1.22), using the matrix form, it is always possible to decompose f 
as: 

f = f r + \f t , 

where 

f r := (° -f) and f, (° « 

The elements f r and T t are both matrices belonging to se(3). It is therefore possible to 
check whether these two matrices are commutative and therefore whether: 

f r f t = f t f r , 

which is true if and only if the Lie algebra commutator (see Appendix B) is zero, i.e.: 

[f r ,f t ] =0. 

This can be easily seen to be satisfied for the given decomposition and therefore, thanks 
to the Campbell-Baker -Hausdorff formula (Selig 1996), it is possible to see that we can 
write: 



e T = e T re XT t =e XT t( ,T r 



With some calculations, it is furthermore possible to see that: 

where R = e w e SO (3) is a rotation matrix. We finally obtain: 

H = H r H t = H t H r , 

which shows that the two motions are commutative as expected. Using wha has been 
explained in Sect. 1.3.1, it is possible to see that H r represents a pure rotation of 
6 := \\uj\\ around an axis passing through r and along uj, and H r represents a pure 
translation of 6* A along the direction uj. We have therefore proven Chasles' theorem, 
since we have shown that any motion can be described as a rotation around an axis 
after a translation along the same axis or vice versa. 
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In the case in which oj = 0, following the same arguments as above, we can see that this 
represents a pure translation along v. 

Furthermore, we have also seen that the six- dimensional vector T can be interpreted 
exactly as the sum of the Plucker ray coordinate vector of a finite line and an infinite 
line: 



where T t is clearly related to T r by Eq. (1.22). The infinite line T t is called the polar of 
the finite line T r , 

This implies that we can express any motion by specifying a finite line, and a scalar A 
which is exactly the interpretation of a screw with an associated module! 

For pure translations we only have a line at infinity which can be easily interpreted as 
a screw with infinite pitch using an argument similar to the one used in Eq. (1.20). 

For a pure rotation we only have a finite line which can be interpreted as a screw with 
pitch equal to zero. 

We have also shown that the linear combination of lines seen as elements of PR 5 is in 
general not a line, but a screw! We can therefore express any linear combination of lines 
as a screw and therefore as a finite line plus the pitch times its polar. 

Note that, as with the line, screws are projective entities which implies that they do not 
in general have a direction and a module. Once we associate a direction and a module, 
we can use them directly to express what is called a finite twist in order to express any 
rigid body motion. In this case, a five-dimensional screw becomes six-dimensional, due 
to the extra information about the magnitude. 



Once again, also for velocities, we can give either a Lie group or a screw interpretation. 
We will analyse them both and find relations as in the case of finite motions. 



1.4.1 Twists as elements of sc(3) 

Elements of se(3) (see Eq. (1.13)) are called twists in mechanics and they represent the 
velocity of a rigid body motion geometrically (i.e., coordinate-free). In order to under- 
stand this, we must look at the action of elements of SE(3) on points of R 3 . Consider 
any point p not moving with respect to the reference frame If we indicate with p l its 
numerical representation in this means that p l = 0. Take now a second reference 
tyj possibly moving with respect to \I>j. By looking at the change of coordinates and 
differentiating, we obtain: 



T = T r + XT t 



1.4 Velocities for a rigid body: Twists 



where we denoted P' 



<k . 




k = i 



and H{ e SE(3). We can now transport H? to the identity either by left or right transla- 
tion. If we do so, we obtain the two possibilities reported in Table 1.1. In the case we 
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Transport 


f 




pj 


Notation 


Left 


f := mki 


H( = HIT 


P J = hktp 1 ) 


i 


Right 


f := Htm 


HI = THi 


P'J = TiH^P 1 ) 


i 



Table 1.1: The used notation for twists. 



consider the left translation, working out the terms, we obtain: 

Hi 

and using the right translation instead we obtain: 

Hi 

From the previous two expressions and Table 1.1, it is possible to see that T^ c repre- 
sents the motion of ^ Q with respect to ^ c expressed in frame ty b . Note that v is not the 
relative velocity of the origins of the coordinate systems! (This would not give rise to a 
geometrical entity.) v is the velocity of an imaginary point connected to the moving body 
and instantaneously passing through the origin at the instant under consideration. 



Changes of coordinates for twists 

Using the left and right map, we have seen that: 

/,'•' //[/'//and/;" ///'//;. (1.28) 

It can be easily seen that 

////;"//:. (1.29) 

and this gives an expression for changes of coordinates of twists. This clearly corre- 
sponds to the adjoint group representation introduced in Appendix B on pag. 70. It is 
easier to work with the six- dimensional vector form of twists and it is possible to see 
that we can find a matrix expression for the adjoint representation: 

V/"' ,\,l u //"'. (1.30) 
It is possible to proof that this matrix representation is: 

Ri 



AdH r-\p> R i ,r 

By differentiation of the previous matrix as a function of H\ we can also find an ex- 
pression for its time derivative and the adjoint representation of the algebra ad (see 
Appendix B). It can be shown that: 

' 3 



Ad,,, .\<l n ad r . with 7? J := H)H\ , (1.31) 
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where 

rl ^ 

adrpk.j :— y~k,j -k d ) 

is the adjoint representation we were looking for. This can be easily checked by testing 
the relation proved in Eq. (B.9) for a general matrix Lie group. It is furthermore easily 
possible to prove that: 

which can be useful to analyse mechanisms. 



1.4.2 Twists as applications on screws 

With the same line of reasoning as we did for finite motions, we can give an interpre- 
tation to elements of se(3) as a twist applied on a geometrical screw. In a similar way, 
we could state Chasles' theorem, but now applied to instantaneous velocities instead of 
to finite twists. Similarly as before, the theorem says that any element of se(3) can be 
described as a pure instantaneous rotation around an axis plus a pure instantaneous 
translation along the same axis: 




rotation translation 



where ui = ||w||w. Analyzing the previous formula and with reference to Figure 1.6, it is 
possible to see that v is the velocity of an imaginary point passing through the origin 
of the coordinate system in which the twist is expressed and moving together with the 
object. The six-vector representing the rotation is what we called previously a rotor and 
can be associated to a geometrical line, namely the line passing through r and spanned 
by lo which is left invariant by the rotation. 
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Figure 1.7: Interpretation of the indices for the notation used in twists. 



Once again, the theorem of Chasles is one of the two theorems on which screw theory 
is based because it gives to elements of se(3) a real tensorial geometrical interpretation. 
This interpretation is the one of a motor or screw which are entities characterized by a 
geometrical line and a scalar called the pitch. This pitch relates the ratio of translation 
and rotation along and around the line. 

If we furthermore consider the change of coordinates expressed in Eq. (1.30), this can 
be interpreted as a change of Plucker ray coordinates of the geometrical line associated 
to the twist using Chasles' theorem. 

This also implies that in the notation t/ c j we use, the frames can be any frame fixed 
to the considered body of reference and this holds also for ^ . 

With reference to Figure 1.7, we can prove what has just been said by noticing that 
since Hi and 7J| are constant, the following is true: 

zjl ZJ~4 ttI rr3 Tj4 rr3 rpl,l 

4 - H A H 1 - H 3 24^3^1 - 1 3 1 

/ 

and that 

ff' 1 = Hlf^Hl = Hf Hi Hi Hi = (HfHl)(HfHl) = ff 2 . (1.32) 

We can therefore talk about Tf as a geometric entity modeled as a screw without the 
necessity to express it numerically in a certain frame ^> k - 



1.5 Forces applied to rigid bodies: Wenches 

The concept of duality is fundamental when working in mechanics. Within the theory 
of Lie groups, we know that se(3) is a vector space, and as such, we can consider the 
dual se*(3) which is the vector space of linear operators on se(3). This corresponds to 
the space of wrenches which are a six- dimensional generalization of three-dimensional 
forces to rigid bodies. 



1.5.1 Wrenches as elements of se(3)* 

Twists are the generalization of velocities and are elements of se(3). The dual vector 
space of se(3) is called the dual Lie algebra and denoted with se*(3). It is the vector 
space of linear operators from se(3) to E. This space represents the space of 'forces' for 
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rigid bodies which are called wrenches. The application of a wrench on a twists gives 
a scalar representing the power supplied by the wrench. A wrench in vector form will 
be a 6 dimensional row vector since it is a co-vector (linear operator on vectors) instead 
than a vector. 

W=(m f) 

where m represents a torque and / a linear force. For what just said we have: 

Power = WT 

where T is a twist of the object on which the wrench is applied. Clearly, to calculate the 
power, the wrench and the twist have to be numerical vectors expressed in the same 
coordinates and result 

Power = WT = mu + fv. 
Another representation of a wrench in matrix form is: 



W 



f m 




How do wrenches transform changing coordinate systems ? We have seen that for 
twists: 

T*' = Ad m T l S 

where 

m 



Suppose to supply power to one body attached to ^ by means of a wrench which 
represented in tyj is W j . 

Changing coordinates from to \ff, the expression for the supplied power should stay 
constant and this implies that: 



w = w j A(i n /;; ' = (A(F H nr ; i' i' /; ' = w i r. 1 

which implies that the transformation of wrenches expressed in vector form is: 

(W l ) T = Ad T H3 (W3) T . 

Note that if the mapping Ad H , was mapping twists from ^ to ^fj, the transposed maps 
wrenches in the opposite direction: from ^ 3 to This is a direct consequence of the 
fact that wrenches are duals to twists. 



1.5.2 Wrenches as applications on screws 

The concept of duality is instead not directly used in the theory of screws. This has 
often brought to useless discussions which could be avoided just by realizing the du- 
ality structure between twists and wrenches. For reasons which will not be explained 
here (Stramigioli, Maschke & Bidard 2000) and depending on the existence of what is 
called the Klein form, the decomposition result for twists given by Chasles' theorem, 
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Figure 1.8: Intuition of a Wrench 



can be given for wrenches by its dual analogous theorem called Poinsot's theorem. This 
theorem states that any element of se*(3) can be split as a sum of two terms: 




(1.33) 



It can be directly seen that we have somehow inverted the the role of the first three 
and last three components with respect to Chasles theorem as shown in Eq. (1.22). 
This fact as profound explanations which can be found in (Stramigioli, Maschke & 
Bidard 2000). The components of Eq. (1.33) can still be interpreted as line components 
which are now called Plilcker axis coordinates. For simplicity, the axis coordinates can 
be just considered as ray coordinates in which we inverted the first and the last three 
components. There are actually much deeper projective geometrical reasons for the 
relation between ray and axis coordinates which have to do with the self duality of lines 
in projective space (Lipkin 1985). 

Also with reference to Figure 1.8, the first element of Eq. (1.33) is representing a pure 
force applied along the line passing through r. The second is instead a pure momenta 
which does not need to be associated to a finite line but to an infinite line which is again 
the polar of the line representing the pure force. 

As we did with twists, we can describe a wrench as applied on a screw. This is true 
because once we have defined a line, a pitch and a magnitude, any wrench can be ex- 
pressed as a line with a magnitude plus the pitch times its polar as shown in Eq. (1.33). 

Note that the role of angular velocities for twists is taken by linear forces in wrenches 
since both elements are associable to a pure line: the first as a rotation axis and the 
second as an application line of a force. In a similar way, the role of pure translations 
is taken by pure moments. 

This also implies that a wrench applied on a screw with zero pitch corresponds to a 
pure force and a wrench applied on a screw with infinite pitch corresponds to a pure 
moment. 
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1.6 Dynamics of a rigid body 

In this section we analyse the dynamics of a rigid body in detail. 

1.6.1 Kinetic co-energy and energy 

Kinetic energy must be expressed as a function of the relative motion of a body with 
respect to an inertial reference. From now on we will consider an inertial reference 
frame ty . 

Even basic courses of mechanics teach that the co-energy with respect to ^ of a point 
of mass m and velocity v with respect to an inertial reference is: 

E k( v ) = ^mv 2 . 

One could wonder why the previous quantity is called co-energy. Formally speaking, 
El is a function of a velocity, and as a function it differs from: 

Mp) = ^-p 2 

2m 

which is called kinetic energy of the mass, where p denotes the momentum (Paynter 
1960, Breedveld 1984). 

We can consider a reference ^ rigidly connected to a body Bi. Talking about rigid 
bodies, we can than consider a constant mass distribution function p : B t — ► M+ which 
associates to each point a mass density. Furthermore, each Euclidean space has an 
?i-form Vl corresponding to the volume form. The mass of a body B t can thus be defined 
as: 

Pi{p)Qi{p). (1.34) 

Bi 

Now consider a relative motion of Bi with respect to The kinetic co-energy of an 
infinitesimal volume element dV in a certain point p e Bi which has velocity v with 
respect to ^ is: 

dE* k = \ P {p)v 2 dV. 

The total energy is nothing else than the integral over Bi . To handle the energy easily, 
we look for an expression of the velocity v which is a function of T/'°, the twist of B L with 
respect to Using coordinates we clearly have: 

?) " (o (? 

This implies that the velocity of the fixed point pi e B t (P l = 0) for the reference * in 
the chosen coordinates is: 

p o) - (5 S) (?) - (? (S s) (? 

where the matrix containing u> and v is T^' , the twist of the relative motion of £?, with 
respect to expressed in ^j. It is possible to see that, after few manipulations: 

h = (-RPi R)t 
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We have shown therefore that for any fixed point pi e Bi there is a linear map from se(3) 
to the velocity of the point p$ with respect to the inertial reference ^o. This also implies 
that the square of the magnitude of P can be expressed as a quadratic form: 



where 



M(Pi) 



\P \\ 2 =T?M(P i )T i 



PfR T RPi Ai? T i?\ _ (If Pi Pi 
-R T RP R T R J \ PT In 



and Ti := (w T v T ) . The matrix M(P l ) can be singular for a single point P L , but the 
sum of M(Pj) for a sufficient number of noncollinear Pi gives a nonsingular matrix. 
A consequence of this is that the integral of this matrix over a nonzero volume would 
always give a nonsingular matrix. Hence the total kinetic co-energy is: 

K{Ti) = \Tfm (1.35) 

where 

/:= / MiPJpiPJQiiPi). 



It can be easily seen that this matrix which is called inertia tensor is symmetric with 
the following form: 

r _ ( Q i P l 
\{P l ) T ml 

where m is the mass of the inertial body. 



1.6.2 Coordinate changes for inertias 

The previous inertia is a tensor and therefore we can analyse how its numerical repre- 
sentation changes if we change the reference system. If we change the coordinates of 



T/' = Ad H i T- fe ' 

we get: 

ji* _ }_(rpk,0^T jk,irpk,0 

where 

I^ = {Ad H] flAd Hl 

is the inertia tensor of body i expressed in frame ^ fe . Note that if H l k is constant also the 
tensor I k > 1 will be constant. It is then well known in mechanics that there exists a frame 
centered in the center of gravity and properly oriented such that the corresponding 
HI gives: 
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where 

Ji = 

The frame ty k is called principal inertia frame for the rigid body. It is easy to see that if 
"J/,, is the principal inertia frame we obtain: 

rj-i* _^rpk,0^T j-k,irpkfi 

II fc,0 1 1 2 , 1 / fc.0\T t fc,0 

2 m ill^ II + 2^ > JlUJ * = 

1 II fc , 1 1 2 , 1 • 2,1- 2,1- 2 

2 m *IK II + 2 JxU} * + 2 JvUJy 2 JzUJz ' 

From this we can conclude that any rigid body Bi of any shape, density and material 
behaves as a uniform ellipsoid centered and oriented in/as ^ k of mass rrii and principal 
inertias j x ,j y ,j z !! 

In any reference, due to the non-singularity of the integral of M(-) and because of 
physical reasons, / is always a positive definite matrix. The physical reason is that if 
we had a singular /, a non-zero twist would exist with no energy associated with it. For 
an object with a finite volume, this is clearly impossible. 

It can be seen that / represents a quadratic form defined in the Lie algebra se(3). It is 
therefore possible to define intrinsically: 

(,)/ : se(3) x se(3) -> R ; (T 1; T 2 ) ^ T[lT 2 (1.36) 

We can then see that the co-energy is equal to: 

The quadratic form / uniquely defines a bijection between elements of se(3) and ele- 
ments of se*(3) such that N z e se*(3) corresponds to T^' e se(3) if and only if: 

N l (T*) = (Jl fi ,T*)i VT* 6 se(3). 

The quantity N\ corresponding through the inertia tensor / to T-' Q , is called the mo- 
mentum, and it corresponds to the twist T- ,Q . 

Remark 5 Note that the quadratic form I in the Lie algebra is a constant quadratic form, 
independent of the relative position of B t with respect to any other space. Therefore it is a 
quantity intrinsically associated with Bi . It is possible to see that, by the group operation 
of left transport, this quadratic form defines a left invariant Riemannian metric in the Lie 
group SE(3). 

Since / is nonsingular, we can define the following function of N\ which will be called 
kinetic energy of the body: 

E k (N i ) = ^(N i ,N i } Y . (1.37) 

Where (, )y denotes the quadratic form onse*(3) corresponding to Y = I -1 . Numerically, 
we have: 

E k (N i ) = E* k (Ti' ) = ±N i Tf> . 
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1.6.3 Euler equation of a rigid body 

For the reasons seen in the previous section, the momenta of a rigid body is defined as: 

(NY := FT*' (1.38) 

We defined N as a row vector because it transforms like a wrench with changes of 
coordinates. This will be clearly shown by the second law of dynamics. It is once again 
a co-vector: a linear operator on elements of se(3). Newton's law for a point mass says 

p° = F°. 

It can be shown that integrating this equation for a complete rigid body, it is generalized 
to 

N°' l = W°'\ (1.39) 

where iV '' is the momenta of body i expressed numerically in frame ^ and W ' 1 the 
total wrench applied to body i expressed in frame ^o- Since the time derivative of 
the momenta is equal to a wrench, both terms of the equation transform identically 
with changes of coordinates and this is why the wrench is a co-vector. If we write the 
previous equation as an equality of column vectors, we have 

and writing out the changes of coordinates we can rewrite it as 

(a<^,(7V«) t ) = (Adl^(N l ) T + Adl h (N l ) T = (W°<y 

Using the result of Eq. (1.31) for the time derivative of the matrix Ad H , we can then 
write: 

adlo,Ad T H ,(NY + Ad T H ,(NY = (W°^) T , 
and multiplying on the left for Ad^o we get 

Ad T H oadl^Ad T H}i {N l ) T + Ad T H oAd T Hh {N l ) T = Ad T H o{W°- l ) T 

ad T Id w i 

which can be written as 

(NY = -adit ANY + (W l ) T 

and since ~T^ 1 = T-'° we have that: 

—adrpi.i = ad = ad^i.o. 

-"o 1 a 1 i 

We have therefore obtained the final result: 

(N l ) T = ad^. (N l ) T + (W l f. (1.40) 

If we furthermore express the previous equation in a principal inertial frame ^> k we get: 

x k T?>°=(-f° :|;°o)^°+(^) t 

which can be written component-wise as: 

t ■ k,0 i t k,0\ . fe.O , / k,i\T 
JuJ k ' = (Juj k ) A U! k + (TO ■ ) 

fe.O fe.O . fe.O . / j-k,i\T 

mv k = mv k Aw fe' +(/ ) 

which is indeed a more common form. 
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1.7 Exponential of Lie algebras 

In what will follow, it is very important to be able to calculate the matrix exponential 
of matrices belonging either to so (3) or even more important to se(3). This is relevant to 
use the presented techniques in practice. We recall that a matrix exponential is defined 
as: 

e A :=I + A+^A 2 +^A 3 + ... (1.41) 
It is possible to see (prove it as an exercise) that the following identities hold: 

1. {e A )- 1 = e~ A 

2. qHAH~ 1 = He A H -i 

3. e A e B = e B e A if and only if AB - BA = 

1.7.1 Exponential of elements of so (3) 

We now start analyzing how the exponential of a 3 x 3 skew-symmetric matrix looks 
like. We will get to the well known Rodriguez formula. Clearly, we can always write an 
element ui of so(3) as: 

lu = 9lu 

where 9 g R and u> is a vector of unit length. By straight forward calculation, it is 
possible to check that 

Co 2 = ujlu t - I\\uj\\ 2 (1.42) 
u) 3 = -w|M| 2 (1.43) 

Using the previous equations recursively, it is possible to see that: 

6 = J+ ('"3! + 5! + -) W+ (2!-4! + -r 

sin(fl) l-cos(e) 

And therefore we finally obtain Rodriguez formula: 

e e " = J + (5sin6l + i5 2 (l -cos6). (1.44) 

1.7.2 Exponential of elements of sc( 3 ) 

It is now possible to calculate the exponential of an element of se(3) which in matrix 
form we have seen to be looking like: 



(1.45) 
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We will consider two cases. The first one is the exponential of a twist whose axis is at 
infinity which we know to be corresponding to a pure translation [u> = 0). In this case, 
it is easy to check that 



v 




Vn > 1 



and therefore we have that: 



^=(» ;)■ ,L46) 

From what we have seen in Sect. 1.4.2, a twist can be interpreted as applied on a 
screw. It is to be expected, that the exponential of a twist expressed in a reference 
frame such that the corresponding screw axis is passing through its origin will be easier 
to calculate. This is exactly what happens. We can therefore translate the coordinate 
system of r where r is the position of the axis of the screw on which the twist is applied. 
We can therefore write: 



uj v\ ( I r \ [ u —ujr + \lu\ f I —r 

oo/loiHo o Mo 1 



(1.47) 



where the middle matrix on the right side is an expression of the initial twist in a frame 
shifted of r and therefore placed on the axis of the screw. By using one of the properties 
of the exponential, it is therefore sufficient to calculate the exponential of 



uj —Cor + \uj 




(1.48) 



By straight forward calculation it is possible to see that the exponential of the previous 
matrix is equal to 



Xuj 
1 



(1.49) 



and re -transforming this expression to the original coordinate system we obtain 

I r\fe Q \uj\fl -r\ fe Q {I - e Q )r + AuA 

o i){o i){o i) = [o i )■ (L50) 

Eventually, substituting the expressions for r and A reported in Eq. (1.25) and Eq. (1.27) 
we finally obtain: 

e (o °)=f e * lfa{V-e?){uAv)+u, T vu)^ (151) 
where e w can be calculated using the Rodrigues formula reported in Eq. (1.44). 



Serial kinematic chains 



The previous Chapter has shown how to use Lie groups and the geometrical 
interpretation of screws in the study of the motion of one single rigid body. This 
Chapter applies that theory to serial kinematic chains of multiple rigid bodies. 
Lie theory of screws provides a very concise and complete description, offering 
more insight than the coordinate -based discussions of traditional textbooks. 



2.1 Configuration kinematics 
2.1.1 Kinematic pairs 

In Chapter 1 we have seen how to describe the kinematics of a single rigid body. We 
will now describe the effect of interconnecting two rigid bodies by means of an ideal 
geometric constraint. Such a constraint is usually called a kinematic pair. For reasons 
of simplicity we will only consider so-called lower pairs, which are motion constraints 
with the feature that their configuration is a Lie subgroup of SE(3). Typical lower 
pairs are prismatic or rotational joints, which are the most common in robotics. More 
complex pairs can be described using differential geometric distributions, but this is 
beyond the scope of this course. 

Important for the description of a constraint between two rigid bodies is what relative 
configurations and motions are allowed, but not what their absolute position is with 
respect to any other body or reference. So, constrained motions are described by relative 
twists; for a lower pair, these relative twists form a constant involutive subspace of se(3). 
(HB: ????) 

Consider two bodies Bi and Bj, and frames and ^j fixed to Bi and Bj, respectively. 
Then consider a relative twist T?' 3 of Bj with respect to Bi (indicated by the leftmost j 
and i indices), as seen by the body Bj (indicated by the rightmost j superscript). All 
possible relative twists T/' J ' form a subspace of se(3). A lower pair is defined to have 
the property that this subspace is the same for all allowed relative configurations Hf of 
Bj with respect to Bi . A coordinate basis of the relative motion subspace consists of a 
matrix T F 6R 6x ° (n < 6 is the number of degrees of freedom of the lower pair): 

e range (7» . (2.1) 

In screw theory terminology, the image of the matrix Tp represents a screw system of 
order n. The revolute and prismatic joints are examples of one-dimensional screw sys- 
tems; a prismatic joint is a two-dimensional screw system; and a plane-plane contact 
is a three-dimensional screw system. 

In geometric terminology, twists belong to the Lie algebra se(3), and Lie algebras always 
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[9, h]\ 




9 



-1 



h 



Figure 2.1: Commutator of two twists (or rather, infinitesimal displacements). 

have a so-called commutator operator defined on them, in addition to the more tradi- 
tional summation operator. (The commutator is often also called the Lie derivative or Lie 
bracket.) In the case of rigid body motion, the commutator [Ti,T k ] of two twists T and 
T fc is shown in Figure 2.1. Give the rigid body B a twist X}, and move it during a short 
period of time; call this motion g. Then give it a different twist T k , and move it again 
during a short period; call this motion h. The third motion is the inverse of the first one: 
move with the inverse twist — T;. Finally, execute the inverse of the motion generated 
by T k . Figure 2.1 sketches this four -motion operation. This composition of four oper- 
ations h~ 1 g~ 1 hg is the commutator of the finite displacements g and h. In general, this 
commutator will not bring B back to its original position and orientation. Now imagine 
that the motions g and h tend to infinite simally small motions, or, in other words, take 
the limit, for the time going to zero, of the commutator divided by the short time period 
during which the motions are executed. This means that the infinitesimal displace- 
ments g and h become tangent vectors to the trajectories, i.e., the above-mentioned 
twists T fc and T ; . Because of the limit process, these velocities apply at the "identity 
element," i.e., the undisplaced pose of the body B. Hence, [Ti,T k ] is a mapping from 
two tangent vectors at the identity element to a third vector at the identity element: 
[•,•] : se(3) x se(3) — > se(3) : v,w 1— > [v,w\. In matrix representation, the commutator is 
represented by a vanishing matrix product: 



The "tilde" notation for twists has been introduced in Eq. (1.21). 

The twist commutator has the physical units of an acceleration. Hence, [T,T fe ] is often 
also called the Lie derivative of T fc with respect to T. (Sometimes denoted with £ T ,Tfc.) A 
Lie derivative has all properties of a derivative (linearity and Leibniz condition), but can 
only be defined for smooth vector fields; a vector field is the assignment of a vector (in 
this case, a twist) at each point of the (relative) configuration space. 

If one has a parallel transport defined on the manifold, this parallel transport also 
transports the definition of the Lie bracket from the tangent space at the identity to the 
tangent space at any other element g of SE(3): first transport the two tangent vectors 
Tfc and T to the identity; then apply the Lie bracket to these two transported tangent 
vectors; bring the resulting tangent vector at the identity back to the original tangent 
space; and define this last vector to be the Lie bracket of T fe and T. Note that the Lie 
bracket in the tangent space at the identity element of a Lie group is an intrinsic feature 
of that Lie group; the Lie bracket at an arbitrary other element of the Lie group depends 
on a rule of parallel transport. For example, left and right translation define different 
Lie brackets. 



[T,T fc ] = [T u T k }=T l T k -T k Ti. 
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Some lower pairs (e.g., a plane-plane contact consisting of two "prismatic" joints with 
axis in the plane and one revolute joint with axis orthogonal to the plane) have the 
property that the commutation of any two possible twists T; and T k vanishes. Physically, 
this means that moving body Bj over the kinematic constraint of the lower pair doesn't 
change its relative motion freedom with respect to body B, L . Other lower pairs (e.g., a 
spherical joint, being three revolute joints with intersecting axes) do not have vanishing 
commutators. 

Every ideal motion constraint between two rigid bodies has two so-called dual sub- 
spaces associated to it: 



• Its n-dimensional twist screw system, with basis T F . 

• Its (6 — n) -dimensional wrench screw system, with basis Wf. 



T F is a subspace of se(3), and Wf is a subspace of se*(3). Both spaces are dual, in the 
sense of linear vector spaces, (Stramigioli 2001). This duality is often called "reciprocity" 
in screw theory. The wrenches in Wf do not generate any power against the twists in 
T F . Or, equivalently, a wrench in Wf cannot transmit energy to the body. Or, in still 
other words, they are the constraint wrenches that the pair can ideally resist without 
actuation. 

Twists of the form 

MS)- -MS 

are called unit twists. A one degree of freedom kinematic pair or joint constraints the 
relative motion of two objects with a unique twist: 

where T?' 3 is a constant unit twist. For a rotational joint 

fjj = ( & 
1 \rAu> 

for any r that connects the origin of the reference frame to a point on the joint axis. If 
the rotation axis passes through the origin of j, than r = 0, and T 3 ' 3 = (lj t 0) t . 

Eq. (1.28) has shown the relationship between the rate of change in the position repre- 
sentation Hi and the twist T 3 ' 3 (in "tilde" matrix representation). If we apply this to the 
motion generated by moving about a joint axis with unit twist T 3 ' 3 and joint velocity it 
and solve the corresponding differential equation, we get: 

H{ (Si) = exp(T- 3 6i) H 3 (0). (2.2) 

Hence, the trajectory Hf(t) generated by applying a constant twist f 3,3 during a certain 
time interval corresponds to the exponentiation of T (Chapter 1). As such, this exponen- 
tial will appear in the kinematics treatment of robotic structures, which are a (mostly 
serial) connection of lower kinematic pairs. 
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Figure 2.2: A serial kinematic chain. 



2.1.2 Forward position kinematics of serial chains and Brockett's 
Product of Exponentials 

Robots are most often used to position a tool (the "end-effector") in the workspace 
("Cartesian space," "operational space"). The desired position (including orientation) 
has to be achieved by moving the motors (mostly rotating drives) to the correct angle 
(these angles form the "configuration space"). The relationship between motor angles 
and end-effector position and orientation is in general non-linear. All non-linear map- 
pings have an "easy" direction, and a "difficult" direction. For serial robots, it is "easy" 
to find the end-effector position when the motor angles are given; that direction of the 
mapping is called the forward position kinematics (or "direct position kinematics"), and 
is subject of this subsection. The direction from end-effector position to motor angles is 
more difficult; this is the inverse position kinematics, discussed in the following section. 

The forward position kinematics are traditionally described by means of an iterative 
numerical procedure: the end-effector position and orientation are found by applying 
the chain rule Eq. (1.11), where each homogeneous transformation matrix corresponds 
to the exponential formula Eq. (2.2): the twist T in exp(Ti) corresponds to a pure rota- 
tional velocity around the motor axis, and Tt is the matrix that corresponds to appying 
the joint twist during a time period t. It is a simple matter to find the (virtual) "time" t 
that the motor had to turn from its zero position to the current position represented by 
the joint angle 6. 

Applying the chain rule 

ttO ttO Tjl rrn—1 

and the exponentiation of Eq. (2.2) to the typical serial kinematic chain of Figure 2.2 
yields: 

where i?| _1 (0) is the position for 4 = 0. 

H° n {e u 6 2 , 6 n ) = e*°'° e i fl?(0)e*» ^(O) ^(0)^(0) . . . e^'^ 6 " H^O), 

V v ' 

e T 2 "2 

or 

-0,0 -0,1 ~0,(n-l) 

fT°(0i,02,...A)=exp(r 1 0i)cxpT 2 6 2 )...cxpT n 9 n )H°(0). (2.3) 
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This result is called the Products of Exponentials (POE) formula, made popular by Roger 
Brockett. This POE formula is attractive from a geometrical point of view, because it 
fully describes the forward kinematics, in a coordinate-independent and concise way. 



2.1.3 Inverse position kinematics 

Formally speaking, the inverse position kinematics problem finds the joint angles 
from the non- linear equation Eq. (2.3), when the end-effector position and orientation 
Hq is given. This text doesn't expand much on this topic, because the most appropriate 
solution technique depends heavily on the particular geometric form of the robot. For 
a robot with six revolute joints, in an arbitrary relative placement, the solution can 
be reduces to solving a 16th order polynomial; most industrial robots have special 
geometrical designs (three intersecting "wrist" axes; parallel second and third axes; etc.) 
that reduce this complexity to simple trigonometric and quadratic problems. Anyway, 
the non-linearity of the inverse problem gives rise to multiple solutions: for each Hq , 
multiple, discrete solution sets (6\,... ,6 n ) exist. If the robot has more than 6 joints, 
one single Hft gives rise to a continuous family of solution sets. Finally, only a limited 
number of Hq is reachable by the robot; this reachable set is called the workspace of 
the robot. 



2.2 Differential kinematics of serial chains 

Very often, not only the position of the robot end-effector is important for the task, but 
also its velocity. The relationship between the velocities of the n motors and the velocity 
(twist) of the end-effector has got the name of differential kinematics, or (forward and 
inverse) velocity kinematics. 

This Section explains the relevant geometric properties: the Jacobian matrix, the sin- 
gularities of the robot, and how to cope with redundancy in the task specification. 



2.2.1 The geometric J acobian 



Mathematically speaking, going from the position kinematics (Eq. (2.3)) to the differ- 
ential kinematics is just a matter of taking the time derivative. The derivative of the 
non-linear POE function results in a linear relationship between the joint velocities 
Oi = d9i/dt and the end-effector twist T™. The matrix that describes this linear relation- 
ship is called the Jacobian matrix, denoted by J(6(t)). 



T[ l = J 



(2.4) 



Although the differential kinematics at any given instant t is a linear relationship, the 
Jacobian matrix itself is a non-linear function of the joint angles. 



It's not difficult to find the differential kinematics from Brockett's Product of Exponen- 
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tials Eq. (2.3): 

^ = Hi [ (f 1 exp(f H)) exp(T H) . . . exp(T"i)} (2.5) 
+ • • • + {cxp(f 4) . . . (f n exp(f n i)) } , 

where f = T0(i). And Eq. (1.28) shows the relationship between the twist Tf of the 
end-effector and the time rate of Hff, i.e., post-multiplication by i?" = (iJJ") -1 . Eq. (1.29) 
then allows to write Eq. (2.5) as (with respect to the first body): 

fn = fi + u 2 f 2 H\ + ■■■ + Elf 2 Hi. (2.6) 



So, we finally arrive at the Jacobian matrix J in Eq. (2.4): 



T[ l = f 1 e\ + f 2 o 2 



T n 9 n = T 1 T 2 



(2.7) 



J 



\0j 



This equation just means that the end-effector twist T™ is the sum of the contributions 
of all joints. The Jacobian is a coordinate basis of the n-dimensional sub-space of the 
tangent space that can be covered by moving the robot's joints (at this particular point 
in time). Its ith column corresponds to the end-effector twist generated by applying a 
unit angular velocity at the zth joint, and no velocities at the other joints. 



2.2.2 Singularities 



A serial robot with n motors spans an ?i-dimensional subspace, with the Jacobian ma- 
trix as coordinate basis. However, because the Jacobian is a non- linear function of the 
configuration parameters 9i, there will be, in general, configurations where the rank of 
J drops, or, in other words, the twists of the different joints become linearly dependent. 
The physical meaning of a singular configuration is that the robot looses a velocity de- 
gree of freedom, or, because of the duality between twist and wrench spaces, it gaines 
a passive degree of constraint. The latter phenomen means that one can exert an ar- 
bitrary large wrench along a certain line without the robot needing any joint torques to 
withstand this load. 

In differential-geometric terms, a singularity is a point where the vector field of joint 
twists "folds." Another way to describe a singularity is as the configuration where the 
set of differential equations of which the joint twist vector fields are solutions, reaches 
a so-called "catastrophy." 



2.2.3 Redundancy 



The "opposite" of a singularity also exists: the robot has more actuated degrees-of- 
freedom than it needs to move its end-effector. (Typically, one calls an n-jointed robot 
redundant, if // > G; however, even a robot with only 4 joints can be redundant for its 
task, e.g., when the task is to place a microphone at a desired position in the work 
space, while the orientation is not important.) 
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Formally, a redundant situation is reflected in the Jacobian matrix having more columns 
than rows. This means that the same end-effector twist T™ can be generated by an in- 
iflnite number of joint velocities 9 L . Or, that the inverse J -1 of the Jacobian J is not 
uniquely defined. The concept of the so-called pseudo-inverse is well known in linear 
algebra, and often used in robotics. Its analytical form is 

Jt = (jTj)- 1 J T . (2.8) 

It is not difficult to show that the pseudo-inverse is the solution of the constrained 
quadratic optimization problem: 

min||(9 r (9||, s.t. T = JO. (2.9) 

This means: of all possible joint velocity vectors 6 that generate the desired end-effector 
twist T, one chooses the vector with the smallest "norm." 

However, in the form of Eq. (2.8), the pseudo-inverse has no intrinsic geometrical mean- 
ing, because the elements of the matrix J T J contain components with different physical 
units, i.e., m 2 /s 2 + rad/ s 2 \ This inconsistency corresponds to the fact that no natural 
norm exists on the space of rigid body motions. On the other hand, most readers will 
intuitively understand that nature solves redundancy in a unique and unambiguous 
way: for example, if one removes all actuation on a 7 degrees-of-freedom robot, then 
it will "fall" according to the laws of physics, and these generate one single solution at 
each different configuration. We will delay the geometric discussion about solving re- 
dundancy until after the following Section, because that requires knowing more about 
the dynamics of serial robots. 



2.3 Dynamics of serial chains 

The previous Section dealt with the kinematics of serial connections of rigid bodies, 
i.e., looking only at the position and velocity descriptions, irrespective of the forces 
that generate these motions. This Section looks at this relationship between forces and 
motion (i.e., acceleration), and extends the discussion about the dynamics of one single 
rigid body to a chained sequence of n rigid bodies. 



2.3.1 The dynamic equations of a serial manipulator 

Sect. 1.6 explained that the dynamics of a single, unconstrained rigid body is the six- 
dimensional generalisation of the three-dimensional dynamics of a point mass; the 
important difference is that the scalar mass property of a point mass is replaced by a 
full 6x6 inertia matrix. 

When connecting the different links of a serial robot, each of these links is constrained 
by the dynamics of the other links. Therefore, the forward and inverse dynamics of an 
n -jointed serial chain are a bit more complicated than the independent concatenation of 
n rigid body dynamics. This section considers the simplest case, as shown in Figure 2.3. 
The constraint generated by the revolute joint can be expressed in two ways: 

• Acceleration constraint: the relative acceleration of both links must be about the 
common joint axis: 

f 1 -f 2 = Ze 2 , (2.10) 



42 



2. SERIAL KINEMATIC CHAINS 



with Z the six- dimensional basis vector of the joint ("unit twist"), and 6 the (as yet 
unknown) acceleration of the joint. (In a frame with its z axis on the joint axis, Z 
has the simple coordinate representation (0 1) T .) 

• Wrench constraint: the transmitted wrench W 2 cannot have a component about 
the revolute joint axis, hence: 

W 2 Z = 0. (2.11) 
(Recall that the coordinate representation for a wrench is a row vector.) 



Because (N 2 ) T = I 2 f 2 (Euler's equation for the dynamics of a single rigid body, Eq. (1.38)), 
with I 2 the inertia tensor of link 2, one finds after premultiplying Eq. (2.10) with Z T I 2 
that: 

Z T I 2 T l = Z T I 2 Zd 2 . (2.12) 
From this equation, 6 can be found, because Z T I 2 Z is always a full-rank matrix: 

§ 2 = (Z T I 2 Z)~ 1 Z T I 2 f 1 . (2.13) 

If both bodies would have been rigidly connected, the force W 1 would have had to 
accelerate the combined inertia I 1 + 1 2 of both bodies, and both bodies would accelerate 
with the same acceleration T 1 , However, the joint filters out part of the acceleration, 
namely the part Z9 2 . Hence, the relationship between wrench W 1 and acceleration T 1 
is: 

W 1 = I 1 t 1 + (i 2 - I 2 Z (Z T I 2 Z)~ 1 Z T I 2 ^jf\ (2.14) 
= l}j\ (2.15) 
with II =I 1 +I 2 -I 2 Z{Z T I 2 ZY 1 Z T I 2 . (2.16) 

I} L the so-called articulated body inertia, (Featherstone 1987), i.e., the increased inertia 
of link 1 due to the fact that it is connected to link 2 through an "articulation" which is 
the revolute joint. The mass of link 2 is "projected" onto link 1 through the joint between 
both links. The corresponding 6x6 projection operator P 2 is: 

P 2 = 1 - I 2 Z {Z T I 2 Zy 1 Z T . (2.17) 

The matrix P 2 is indeed a projection operator, because 

P 2 P 2 = P 2 . (2.18) 

The total articulated inertia of link 1 is the sum of its own inertia I 1 and the projected 
part P 2 I 2 of the inertia of the second body: 

I\ = I 1 + P 2 I 2 . (2.19) 

Note that the projection operator has the form of the pseuso-inverse in Eq. (2.8), which 
is also a well-known fact from linear algebra. The geometrical interpretation of this fact 
is that the (articulated) inertia acts as a natural metric on the configuration space of a 
serial kinematic chain: it defines the "orthogonal projection" of physical quantities. It 
also acts as a natural identification of the (acceleration) twist and wrench spaces: each 
wrench gives rise to a well-defined acceleration twist, and vice versa. 




Figure 2.3: A rigid body is connected to another rigid body by a revolute joint. The joint 
cannot transmit a pure torque component about its axis, generated by the external 
forces. 



The previous paragraphs explained the projection of the inertia tensor. The correspond- 
ing projections of acceleration and wrenches follow straigthforwardly: 

f 2 = (l - Z (Z T I 2 Z)~ 1 Z T I 2 ^ f\ (2.20) 

= {P 2 ) T f 1 . (2.21) 

and 

W 1 = W 2 - I 2 Z(Z T I 2 Z)- 1 Z T W 2 , (2.22) 

= P 2 W 2 . (2.23) 

These link-to-link relationships are the basis of recursive algorithms to find the forward 
and inverse dynamics of serial robot, but this fall beyond the scope of this text. 



2.3.2 Redundancy resolution 



The kinematic section on redundancy resolution, Sect. 2.2.3, introduced the pseudo- 
inverse formalism for redundancy resolution. But the (J T J) _1 J T form is physically 
inconsistent. However, the inertia-weigthed projection matrices of the previous Section 
do have a clear physical interpretation, and it is not difficult to show that they occur as 
the solution of the constrained and weighted quadratic optimization problem that is a 
generalization of Eq. (2.9): 

min||0 T J e 0||, s.t. T = JO, (2.24) 

where I 9 is the robot's inertia tensor, expressed in joint coordinates. The physical 
interpretation of Eq. (2.24) is: of all possible joint velocity vectors 9 that generate the 
desired end-effector twist T, one chooses the vector with the smallest kinetic co-energy. 
(Compare Eq. (2.24) to Eq. (1.35), which expresses the same co-energy in Cartesian 
space.) This kinetic co-energy weighting is often used in robotics publications, but few 
understand that it is the unique choice that nature makes itself. 



2.3.3 Ideal constraints 



The previous Sections assumed that the robot's end-effector was moving unconstrained 
in free space. What changes if the end-effector is making contact with a rigid surface, 
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and hence looses one or more degrees of freedom? (This section considers ideal con- 
straints only, i.e., frictionless and infinitely rigid constraints.) Conceptually, the dis- 
cussion runs along exactly the same lines as in the Sect. 2.3.1: the constraint on the 
end-effector is represented by a wrench space of ideal constraint forces, and the Z vec- 
tor is to be replaced by a basis of this wrench space. Formally, nothing else changes in 
the expression of the projection operators. 

So, when in contact with an environment that constrains its motion according to a 
wrench space with basis Z (we abuse the same notation here to represent a more than 
one-dimensional subspace), the robot's end-effector twist T should be such that it min- 
imizes its internal kinetic co-energy while respecting the motion constraint: 

min||T T 7T||, s.t. ZT = 0, (2.25) 

where I is the robot's total inertia tensor. 

The projection operator P that can be constructed from this optimization problem (just 
as in Sect. 2.3. 1) has been used in so-called hybrid force / 'position controllers, (De Schut- 
ter, Torfs, Dutre & Bruyninckx 1997, Raibert & Craig 1981). These controllers are based 
on the observation that an ideal constraint reduces the motion degrees of freedom of 
the end-effector from 6 to n < 6. Hence, only n degrees of freedom should be "position" 
controlled, and the other 6 — n degrees of freedom are "force" controlled. The "hybrid" 
controller then consists of the sum of contributions from independent, lower-than-six- 
dimensional position and force controllers. And the six- dimensional force and motion 
measurements are projected on these lower -dimensional subspaces by means of the 
above-mentioned projection matrices. 



Interaction and Control 



This Chapter introduces what is called the IPC-Supervisor scheme for robot 
control (Stramigioli 2001). Within this control paradigm, robot control is not 
any longer a classical input-output system, but rather an energy-exchanging 
interconnection of the robot and the controller (which is itself interpretable as a 
physical system). The advantages of the method are robustness and intuition 
in control design. The mathematical foundations of the control approach are, 
on the one hand, classical Hamiltonian theory of dynamic systems, and on the 
other hand, the interconnection theory of mechanical networks. 



3.1 Ports and Interconnection 

In this section we will describe the basic ideas and tools which are used in the proposed 
approach. We introduce power ports and Port Controlled Generalized Hamiltonian Sys- 
tems, as alternatives to the more traditional input-output control paradigm. The latter 
has produced controllers such as hybrid position/ force control, which relies on the 
projection operators of Sect. 2.3.3. The disadvantage of this type of controllers for sys- 
tems that are interconnected to each other is that it is often not possible to specify, for 
example, desired velocities as inputs, because the robot might be constrained by an 
(unknown) rigid wall. The control approach in this Chapter is not of the input-output 
family, but starts from the energy flow in the network of interconnected systems; forces 
and motions will be the result of specifying the behaviour of the total system. These 
interaction controllers have very thorough geometrical descriptions, that generalize the 
concept of a Lie group/algebra of the previous Chapters. 



3.1.1 Power Ports 

A basic concept which is needed to talk about interconnection of physical systems is 
the one of a power port (Maschke, van der Schaft & Breedveld 1992), (Stramigioli 1998). 
With reference to Figure 3.1, a power port is the entity which describes the media by 
means of which subsystems can mutually exchange physical energy. Analytically, a 
power port is the Cartesian product of a vector space V and its dual space V* : 

P := V x V*. 

Therefore, power ports are pairs (e, /) e P. The values of both e and / (effort and flow 
variables) change in time and these values are shared by the two subsystems which are 
exchanging power through the considered port. The power exchanged at a certain time 
is equal to the intrinsic dual product: 

Power = (e|/). 
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(T,W) € se(3) x se*(3) 
Figure 3. 1: The interaction between two mechanical systems 



This dual product is intrinsic in the sense that elements of V* are linear operators from 
V to R, and therefore, to express the operation, we do not need any additional structure 
than the vector space structure of V. 

To talk about the interconnection of mechanical systems, a proper choice is V = se(3), 
the space of twists and V* = se*(3) the space of wrenches. 



3.1.2 Generalized Port-Controlled Hamiltonian Systems 

Dynamical systems have traditionally had two exchangeable theoretical approaches, 
the Lagrangian and the Hamiltonian approach. In the context of this Chapter, where 
the exchange of energy is the key physical phenomenon, the Hamiltonian framework is 
the more natural, because it uses the total energy of the system as its key component. 

In standard Hamiltonian theory, the starting point is the existence of a generalized 
configuration manifold Q. Based on Q, its co-tangent bundle T* Q is introduced which 
represents the state space (or "phase space") of configuration ("position") and momentum 
pairs (q,p). T*Q can be given a natural so-called symplectic structure, on which the 
Hamiltonian dynamics can be expressed (Arnold 1989). 

A symplectic structure on a manifold M is a closed "2-form". A one-form is another 
name for an element of a co-tangent space, i.e., a mapping that takes a tangent vector 
as input, and gives a real number as output; a two-form is an anti-symmetric mapping 
that takes two tangent vectors as inputs, and gives a real number as output. An n-form 
is closed if it is the derivative of an n — 1-form. A wrench is "closed" (and therefore exact 
using Poincare' lemma) if it is the derivative of a potential function; in other words, it is 
conservative. 

Without going into details, the reader should recall from these paragraphs that every 
co-tangent space has a natural symplectic form; the corresponding tangent space does 
not have this structure. This is another fundamental difference between twists (i.e., 
tangent vectors) and wrenches and momenta (i.e., co-tangent vectors). 

A limitation of the Hamiltonian approach is that, (by construction), the dimension of 
the state space T*Q is always even. Moreover, it can be shown that, in general, the 
interconnection of Hamiltonian systems in this form does not result in a system of the 
same form. 

These problems can be overcome with the more general Poisson structure, (Olver 1993), 
or, even more generally, using Dirac structures (Dalsmo & van der Schaft 1999). A Pois- 
son structure is an additional structure on any state manifold. The Poisson structure 
is the mathematical representation of a kind of network structure and the manifold is 
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the total state space of a system. With network structure it is meant that this structure 
specifies how energy can flow from one part of the system to another. 

If this state space is a dual Lie algebra on which a left invariant metric is defined, 
as in the case of the momenta (the state) of a rigid body and its inertia tensor (the 
left invariant metric) , a natural Poisson structure can be defined which is called a Lie- 
Poisson structure. 

A Poisson bracket {•,•} is a binary operation {/, g} on functions f and g of the state 
space. For such an operation to be a Poisson bracket, it should satisfy the following 
properties: 

1. (bi-jlinearity: {af + (3g, h} = a{f, h} + /3{g, h}. 

2. anti-symmetry: {f,g} = -{g,f}. 

3. Jacobi identity: {{/, g}, h} + {{h, f},g} + {{g, h},f} = 0. 

4. Leibniz identity: {fg, h} = f{g, h] + g{f, h}. 



If all the previous properties are satisfied except Jacobi identity, we talk about a gen- 
eralized Poisson structure. It can be shown (van der Schaft 2000) that this structure is 
needed for modeling mechanical systems with nonholonomic constraints. The informa- 
tion about the constraints is included in the structure and this is different than in the 
Lagrangian approach. 

Even if the Poisson structure is properly represented by a Poisson bracket on functions, 
the best way to understand it is by using what is called the corresponding Poisson ten- 
sor. As a quadratic form maps two vectors (fields) to a scalar (function) , a Poisson tensor 
maps two co-vectors (fields) to a scalar (function). A typical co-vector is the differen- 
tial of a function. In our case this function is the energy of the modeled system which 
is clearly a function of the state of the system. Furthermore, for the anti-symmetry 
property, this Poisson tensor should be skew-symmetric. Once we have chosen a chart 
for the state manifold under consideration, we can represent the Poisson bracket as 
follows: 

{F{x),H{x)}{x) = — J{x) — . 

Clearly, since {F{x),G{x)}{x) is a function and since ^ is a 1-form (co-vector field), the 
vector 

ox 

will be a vector field. If H(x) is the Hamiltonian of the system (the energy function), this 
vector field is called the Hamiltonian vector field of the system. Clearly this vector field 
leaves the Hamiltonian invariant (constant) due to the skew-symmetry of J(x) : 

An autonomous Hamiltonian system is therefore completely described by an Hamil- 
tonian function and a Poisson structure and the corresponding equations using the 
Hamiltonian vector -fields just explained are: 

B H 

x=J{x)—. (3.1) 
ox 
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Even more general structures than the Poisson structures called the Dirac structure 
have been defined. The Dirac structure can cope with the modeling of implicit Hamil- 
tonian system. The Poisson structure only with explicit Hamiltonian systems. Some 
introductory material on Dirac structures for modeling Hamiltonian systems can be 
found in (van der Schaft 2000). For more specific applications to mechanical systems 
and robotics the reader is referred to (Stramigioli 2001). In Eq. (3.1) the system is au- 
tonomous and therefore it cannot exchange energy with the rest of the world. It is then 
possible, using the concept of power port introduced in Sect. 3.1.1 to extend Eq. (3.1) 
to what is called a Port Controlled Hamiltonian System (PCHS) (Maschke et al. 1992) : 

x= J(x)^^ + g(x)u (3.2) 

y = g (x) dx , (3.3) 

where u is an element of a vector space V and the external input to the previously 
autonomous system, J(x) = — J T (x) is the skew- symmetric Poisson tensor which can 
be seen to describe in general the network structure and interconnection of the energy 
flows within the system, g(x) is a representation of the interface of the system with the 
rest of the world, and y is the representation of an element belonging to the dual vector 
space V*. Clearly (u, y) is a power port since it belongs to V x V* and it will be shown 
shortly that y T u corresponds to the power supplied to the system.. 

The Jacoby identity for the Poisson bracket corresponds to the following condition for 
the Poisson tensor: 



E 

i=i 



j^ + j^ + J ^ dJkt 



dx l dx 1 dx 1 



It can be shown that conservative mechanical systems with non-holonomic constraints 
can be nicely modeled by Eq. (3.2) where J(x) does not satisfy the previous condition. 
The most general form of an explicit, conservative, Hamiltonian system is furthermore: 

x = J{x)t&- + g{x)u (3.4) 
y = g T (x)^^ + S(x)u (3.5) 

where S(x) is a skew-symmetric quadratic form on the vector space V depending on the 
state x. 

To account for dissipating elements, it is possible to see that we can generalize the 
previous form considering a symmetric, semi-positive definite, two covariant tensor 
R(x) which can be subtracted from J(x): 

x = {J{x)-R{x))^^-+g{x)u 

y = g T (x)^l + S(x)u. (3.6) 



With this new term, it can be seen that the change in internal energy is: 

u t (dH\ T 3H 

supplied power 



dx J dx 



dissipated power 
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Figure 3.2: A simple example of interconnection 



Since R(x) is positive semi-definite, this implies that the internal energy can only 
increase if power is supplied through the ports. Eventually, it has been shown in 
(Stramigioli, van der Schaft, Maschke, Andreotti & Melchiorri 2000), that an even more 
general form is necessary for tele -manipulation applications. This form is: 

x = (J(x) - R( x ))^l + g( X ) U 

y = g T ( X )^^- + (S(x)-B(x))u. (3.7) 

where the new positive semi-definite quadratic form B(x) on V is necessary in order to 
model the impedance adaptation to a communication line. 

As an example, consider the interconnection shown in Figure 3.2 of a mass representing 
a robot with the physical equivalent of a controller implementing damping injection as 
introduced in (Stramigioli, Maschke & van der Schaft 1998). 

The Generalized Hamiltonian model of the "robot" is: 

± \ = f° 1 \{ ^ + f° °\ f Fext 
p] {-1 0) \p/m) + \l l)\ F c 

x\ fO l\ / 



p J \0 1 J \ p/m 

where the port {u\,y\) = (F ext ,x) represents the interaction port of the robot with the 
environment and (112,2/2) = (F c ,x) the interaction port with the controller. The energy 
function is H(p) = -^p 2 where p is the momentm 1 . The physical system representing 
the "controller" of Figure 3.2 can be instead represented using the following Generalized 
Hamiltonian equations with dissipation: 




where the port {u\,y-\) = (x, —F c ) is used to express the interconnection with the "robot" 
and the port (1/2,2/2) = {x v ,—F v ) is used to express the interconnection with another 



: It Is important to notice that imti 2 is properly speaking called co-energy instead of energy because it is a 
function of v which is not a physical state extensive variable (Breedveld 1984). 
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system which turns out to be the supervisory module. The stored energy is: 

H(Ax c , Pc ,Ax) = -k c Ax 2 c + \kAx 2 + —p 2 c . 

2 2 Am 

It is shown in (Stramigioli et al. 1998) that by choosing m c <C m and k > k c for the 
controller, damping injection can be implemented with pure position measurements. 
Furthermore, actuator's saturation can be handled in a physical way by choosing a 
non linear spring k. 

Any physical system can be modeled in the same way and this is the power of GPCHS 
and their importance to describe the proposed architecture. 



3.1.3 Interconnection of GPCHSs 



A very important feature of GPCHSs is that their interconnection is still a GPCHS. To 
show this, consider two GPCHSs: 

*=M-R0§| + (tf «?)($)> (3 - 8) 



The two systems can be interconnected through the interconnection ports (u{,y{) by 
setting: 

u[ = y T 2 and u{ = -y{ (3.10) 

Note that the minus sign in the previous equation is necessary to be consistent with 
power: Pj = (u(,y-) is the input power of system i and, interconnecting the system 
through the "I" ports, we clearly need Pi = — P 2 - It is possible to see that the intercon- 
nected system results in: 

x = (J(x) R(x))^-+g(x) , (3.11) 



where x = {xx,X2) J ', H(x) = Hi(xi) + H 2 {x2) (the sum of the two energies), 



J(x)-( Jl 9 ^ T \ 



J?i o 
R 2 



R(x) = 

and 

9( x ) 

where all dependencies of the matrices have been omitted for clarity. 



9?" 
9?' 



It is possible to conclude that the interconnected system is therefore again a GPCHS, 
with as ports the remaining (i.e., not yet interconnected) ports. Furthermore the total 
energy is the sum of the energies of the two systems. 
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Figure 3.3: A biological analogy of the control strategy 




Figure 3.4: The System Interconnection 



3.2 The proposed control architecture 



The main idea of the proposed scheme is to divide the control in two parts, one which 
controls the real time interaction passively and is called Intrinsically Passive Control 
(IPC), and one which takes care of the task decomposition and other planning issues. 
As shown in Figure 3.3, the IPC has the role of the muscle spindles in biological systems, 
and the supervisor the neurological role of the brain. 



The IPC is interconnected with the supervisor and the robot through power ports as 
shown in Figure 3.4 where bond-graph notation is used. Each bond is representing 
two elements belonging to a vector space (effort) and to its dual (flow) as explained in 
Sect. 3.1.1. With reference to Sect. 3.1.1, the power port corresponding to the inter- 
connection with the robot is characterized by the vector space V being T q Q where Q is 
the robot configuration manifold and q is the current configuration and therefore the 
elements of the ports will be pairs of the form (q, r) where t are the joint torques. 

The other port of the IPC will be connected to the supervisor and in general will have a 
geometric structure such that: 



corresponding to a set of twists. This is the case in both (Stramigioli, van der Schaft, 
Maschke, Andreotti & Melchiorri 2000) and (Stramigioli, Melchiorri & Andreotti 1999). 



3.2.1 The IPC 



^ = se(3) x ... xse(3) 
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Figure 3.5: A tele-manipulation setting. 



The IPC will therefore be characterized by the following differential equations: 



x = (J(x) - R(x))^ + g(x) 



Ti 



\T n ) 



W 1 



\w n ) 



\T n J 



(3.13) 



(3.14) 



where J(x) is skew symmetric, R(x) positive semi- definite, T\,...,T n are a set of in- 
teraction twists and Wi,...,W n a set of the dual interaction wrenches. It has been 
shown in (Stramigioli, van der Schaft, Maschke, Andreotti & Melchiorri 2000) that the 
feed-through term B(x) is needed in tele-manipulation to adapt the impedance of the 
line. 



3.2.2 The Supervisor 

The Supervisor must plan the task, and schedule the subsequent sub-tasks, and plays 
the role of "brain" in a human analogy. 

Following the grasping example of the previous section, to grasp and move an object, 
we have to 

1. Open the hand 

2. Move around the object 

3. Close the hand (grasp) 

4. Move the object 

5. Open the hand 



All this subtasks can be implemented by means of control signals to the IPC by the 
Supervisor which can supply a controlled amount of energy to the system in order to 
perform useful tasks. 
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A Tele-manipulation setting 

In a tele -manipulation setting, the presented architecture is still valid, but the role of 
the supervisor is taken over by a human on the other side of a transmission line. 

A perfectly bilateral tele -manipulation system using the presented architecture is re- 
ported in Figure 3.5. This system has been studied and implemented experimentally in 
(Stramigioli, van der Schaft, Maschke, Andreotti & Melchiorri 2000). 

The environment on one side is the environment to be manipulated and the one on the 
other side is the human which manipulates the system remotely. 

The port variables are transformed to scattering variables (block Z in the figure) to 
preserve passivity even with time-varying time delays due to the transmission line 
(Stramigioli, van der Schaft, Maschke, Andreotti & Melchiorri 2000). The supervisor 
on one side is in this case composed of the transmission line, the IPC, the Robot and 
the "Environment" of the other side. 

Once again, due to the consistent framework using power ports, passivity is preserved 
in any situation. 

Some inconsistencies where present in (Stramigioli, van der Schaft, Maschke, Andreotti 
& Melchiorri 2000) due to a non intrinsic choice. These inconsistencies have been 
solved in (Stramigioli 2001). 



3.3 IPC Grasping 

In this section we present and discuss, as an example, the application of an Intrinsically 
Passive Control (IPC) strategy to robotic grasping and manipulation tasks. Major advan- 
tages of the presented control strategy are the physical intuition on which it is based, 
its passive nature, and the ensured stability for the overall system in all the situations, 
including in particular the transition from no-contact to contact and vice-versa. One of 
the main features of the proposed control is that only joint position measurements are 
needed. This means that no velocity or force measurements are required in the control 
loop, simplifying the sensorial complexity and enlarging the possibilities of application 
of the scheme. 

In general, the environment a robot interacts with must be characterized from a geo- 
metrical point of view, i.e. the dimensions and location of the object to be handled must 
be known in advance. This knowledge is used for planning the grasp or manipulation 
phases, e.g. approach, contact, force application and so on. On the other hand, from 
a mechanical point of view the information on the object/environment, if available, are 
often poor or not precise, e.g. mass and friction properties are in general not exactly 
known a priori and therefore cannot be used for the task planning. 

For this reason, additional sensors (force, tactile, ...) must be introduced and used in 
real time, and the planned grasp should be robust enough to ensure a proper behav- 
ior, i.e. the safe achievement of the grasp /manipulation, for different materials and 
grasping configurations. 

From the control point of view, several control and task planning strategies have been 
proposed in the literature in order to execute grasps and manipulations with a robotic 
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Figure 3.6: Basic idea of the proposed IPC. 



system, see e.g. (Murray, Li & Sastry 1994) among many others. Concerning these 
control schemes, it may be noticed that very few among them consider explicitly the 
problem of controlling, or even defining, the dynamics of interaction. In addition, one 
of the most problematic phenomena in force control strategies is that stability cannot 
be ensured if not assuming as known important features of the object to be grasped, 
like its stiffness and friction. Furthermore, a force control strategy alone is not suitable 
to properly control the transition between no-contact and contact. This is due to the 
obvious fact that force control is meaningful only in contact, since no force can be 
exerted in free space. For these reasons, a control strategy for grasp and manipulation 
based on physically-based observations and on passivity concepts seems worth to be 
pursued. 

This strategy does not have the shortcomings of other grasping control techniques. In 
particular, it is strictly passive in steady-state while during manipulation the supplied 
energy is directly controllable. Moreover, the related compliance control of each finger 
allows for rolling, slipping, and whole-hand grasps in a natural way. 

In other terms, the technique illustrated in this section allows to shape the potential 
energy of the robot/object system in order to achieve a desired compliance, and injects 
damping to ensure both asymptotic stability and proper transient behavior. The main 
point here is to study the interaction between the robot and an object (the environment) 
in such a way that the overall system is stable independently on partially unknown 
geometrical or mechanical properties, and also achieves desired performances during 
task executions. 

This section is concerned with the design of the IPC part of the controller. Since the 
IPC has been conceived in order to have a physically interpretable behavior, it will be 
described here in terms of spatial interconnection of physical elements like springs, 
dampers and inertias. As a matter of fact, the basic idea of the IPC is shown in Fig- 
ure 3.6, where bodies mi,ra 2 -- - ,m n represent the last links of some kinematic chains, 
e.g. the fingertips of a robotic hand or the distal links of robot arms. Both the body with 
mass m& (assumed to be rigid) and the springs (with stiffnesses ki, &2, • • ■ ,k n and k v ) are 
implemented directly by means of control and are exploited, along with the damping b, 
in order to define the dynamic behavior of the IPC and consequently of the overall sys- 
tem. In the control algorithm, the dynamics of mass m b (called here the virtual object) 
is simulated in real-time. In particular, since it is subject to the forces generated by 
springs kx, k 2 , . . . ,k n and by the damper b, its motion in 3D is computed in real time. 
Although this mass does not physically exist, it is of major importance for the control, 
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since its "kinetic energy" can be dissipated by the damper b, achieving in this manner a 
passive behavior for the control. 

Note that the computation of the damping force due to b needs only the knowledge of 
the velocity of the virtual mass nib, that is known since it is computed in the controller. 
If dampers had been attached to masses mi,... ,m n , their velocities would have been 
necessary to simulate their behavior, with the necessity of joint velocity measurements 
for the robot. 

The stiffnesses ki,...,k n and k v of the springs, their rest lengths, the mass m b of the 
virtual object and the position x v are parameters to be used in real-time to achieve 
the desired behavior from the system. Obviously, it should be taken into account that 
careless changes of these parameters can result in a non passive behavior due to an 
apparent energy change of the system. More sophisticated methods to use variable 
springs can be found in (Stramigioli & Duindam 2001). 

In the following sections, formal models of these physical entities and of their intercon- 
nection is discussed. This model is the basis of the proposed control strategy. 



3.3.1 The Springs - Spatial Compliance 

A spatial (3D) compliance is a geometric spring connecting two rigid bodies B t and Bj . 
Loncaric in (Loncaric 1985, Loncaric 1987) studied geometric springs represented 
by potential energy functions of the relative position of the rigid bodies to which they 
are attached. Successively (Fasse 1997, Fasse & Breedveld 1998a, Fasse & Breedveld 
1998b) extended this work giving some useful geometrical parameterizations. More 
recently, (Stramigioli 1998) extended the formulation to a completely coordinate-free 
setting and also in order to consider elastic elements with more than two ports and 
variable lengths. 

A spring between two rigid bodies B t and B. } is characterized by a positive definite 
function representing the stored potential energy 2 with the following form: 



where H{ G SE(3) is the matrix representing the isometry which brings a chosen refer- 
ence frame tyj (fixed to body Bj) to another reference frame (fixed to body B^ 3 . 

Once a potential energy has been defined, the corresponding "force" generated by the 
elastic potential can be computed by considering the differential of Vij : 



which is the local force that body Bi applies on the spring Vij in the relative position R\: 
dV i: j(Hl) £ T^ j SE(3). This local force can be seen as the generalized force corresponding 
to a parameterization of SE(3) like Euler angles and translation positions. 

The wrench applied on the spring connecting B t to Bj by body Bi and expressed in 
frame ^j, with a relative position H{ is: 



2 This function defines implicitly the unit of energy. 

3 Note that is also the matrix expressing the change of coordinates from 4% to j , but the corresponding 
motion is given by its inverse (Hj) -1 . 



V id : SE(3) K; //;' 



dVij : SE(3) -> T*SE(3) 
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where - e R 3x3 is a skew-symmetric matrix corresponding to the force f-j e M 3 using 
the tilde notation introduced in Eq. (1.1). In the previous notation, we have indicated 
with the wrench applied to a spring element connecting * t to tyj on the side of ^ 
expressed as a numerical vector expressed in ^ . 

We can also associate to the wrench matrix W( } e M 4x4 the corresponding vector rep- 
resentation which we indicate with W?j := {n{ J •) G M. 6 . It can be seen that(W / *) T = 
— (Wl j) T = —AdP^j (W/j-) T is the wrench that the spring applies to body _B, expressed in 
the frame and that (W!j) T = -{yV 3 j i ) T = —AdF Hi (W]f) T is nothing else than the wrench 

that the spring applies to body Bj expressed in frame and furthermore W l . J = —Wj ti 
due to the nodicity of a spring. 

A desired energy function can be denned (and implemented using control) such that the 
relative configuration Hf = h corresponds to a minimum of the potential energy Vij(-) 
(Fasse 1997). In this configuration, the frames ^ and ^ will coincide. The energy 
function can be chosen such that the common origins of ty l and tyj in the equilibrium 
position represent the center of stiffness (Stramigioli 1998). Expressed in the equilib- 
rium frame (^j = *_,), we can then choose three 3x3 desired stiffness matrices K a , 
K t and K c , corresponding respectively to the orientational, translational and coupling 
stiffnesses. From these stiffness matrices, we can calculate the so-called co-stiffness 
matrices (Fasse 1997) G a , G t and G c related to the stiffnesses matrices by: 

G a = itr(A' Q )J- K a 

where a = o,t,c and where tr() is the tensor trace operator. It is then possible to give an 

/ ]f) pi 

expression of the wrench W l t as a function of the relative configuration H{ = ( * \* 
(see (Fasse 1997)): 

Wl(Hl) = (ft "fj where 

4 = 2 aMf ;,./,';) - as(G t i?^'^) - 2as{G c piRi) 

ft = -R) 'ds(G t v\)R\ - ;»<C, Hji'i l>'i ) -2as(G c R{) (3.15) 

and where as( ) is an operator which takes the skew- symmetric part of a square matrix 
and the 'tilde operator' is defined in Eq. (1.1). 

Eq. (3.15) is an expression that can be directly used for the implementation, but as 
previously remarked, it is possible to give a GPCHS form of a spring (Maschke 1996): 

//; r; J (3.16) 

W ( =R TjYhl. (3.17) 

where hi is a six dimensional local coordinate of SE(3), J(h{) = 0, and Rh(-) represents 
the Lie group right translation in the chosen coordinates h. It is also possible to consider 
an additional power port which can be used to vary the effective rest length of a spring 
as shown schematically in Figure 3.7. By using an additional port, the relative position 
of b and i can be modified. The obtained effect is to change the rest length of the spring 
between b and j. Applying in the general 3D case the concept of Figure 3.7, the twist 
relation becomes: 

T 3,3 = T 3,3 + AdjjlT^ (3.18) 



3.3. IPC GRASPING 



57 



7 



b i j 

Figure 3.7: Schematic drawing of a power consistent variable spring 



In this case, its GPCHS representation is: 



/<;' I''', -HJ fill) (3.19) 



where the port (T b ' b , W b b ) can be effectively used to change the rest length of the spring. 
More details on the above concepts and mathematical derivations can be found in 
(Stramigioli 1998) and (Fasse 1997). 



3.3.2 Masses 



As it was shown in Sect. 1.6, the dynamic properties of a rigid body are uniquely de- 
scribed by its inertia tensor I b . Let us consider a uniform sphere B b , and a reference 
system ^ b fixed to it with origin coincident to the center of the sphere. We have seen in 
Eq. (1.40) that the equation of the rigid body can be expressed as: 

(N b f = adl b , {N b ) T + {W b ) T ■ (3-20) 

1 b 



It can be shown that: 



(N i ) T = ad^, (N i ) T + (W i ) T ^ 

<**>'- (T :$»)((^) + < H "> T -- 
«*♦>*-(! %){$)^~ 

(N l ) T = (N i A)T^'° + {W l ) T 

and therefore we can write the equations as 

{N b ) T = {N b A)T b b ° + {W b ) T 

where where (iV b ) T = I b T b0 e K 6 is the generalized momentum, T b '° = ((uj b b °) T (v b b °) T ) T 
is the twist of the sphere respect to an inertial frame ^o, W b tot is the total wrench applied 
to the sphere and N b A comes from the Lie-Poisson bracket and in the body coordinates 
ty b is represented by a 6 x 6 matrix of the following form: 



(N b A) := 
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where N b = (N u N v ). For the specific case of the sphere, I b = ( ^ 3 ^ ) where 7 3 is 



mis 

the 3x3 identity matrix, j is the rotational inertia of the sphere and m its mass. 

Also in this case, it is possible to give a GPCHS representation of the inertia's dynamics: 

(N b ) T = (N b A)^±+ (W b f (3.21) 

b - g N b 

where E k (N b ) = \N b {I b )~ 1 {N b ) T is the kinetic energy which is a function of N b instead 
that a function of T b '° as usually thought. The corresponding function E'l(T b ' ) is called 
co-energy. 



3.3.3 Dampers - Energy Dissipation 

The easiest manner to model a linear spatial damping effect is to use an element which 
generates a wrench directly proportional to the twist of the body whose free-energy has 
to be dissipated. In this presentation we use: 

«) T d iss = RT b b ° (3.22) 

where R e R 6x6 is a positive definite matrix representing a dissipation tensor in the 
frame ^ b . 

Note that this element does not have a state and it will appear in the complete GPCHS 
of the interconnected part, as a part of the tensor R(x) of Eq. (3.6). 



3.3.4 The control Scheme 



Following the steps presented in (Stramigioli 1999, Stramigioli 2001), it is possible to 
give a GPCHS description of the controller in the following form: 

(3.23) 




where x B ,xs are respectively the states of the virtual object and the springs as presented 
in Eq. (3.21) and Eq. (3. 19), H c (xb,xs) is the the sum of the kinetic energy of the virtual 
object plus the potential energies of the springs, and fa, Jb and Rb are properly defined 
matrices (Stramigioli 1998). The interaction ports are the pairs (t°, b y w°, b ^) and 

(4ar> «&r) where 



t° = 



, T o,<A 
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Figure 3.8: The interconnection between the robot and the intrinsically passive con- 
troller. 



is the vector of twists of the fingertips with respect to the inertial frame, t®,~ is the twist 
corresponding to the motion of the configuration x v in Figure 3.6 and it is used by the 
supervisor to change the virtual position of the hand and 




is the vector of twists that can be set by the supervisor to change the minimum potential 
configuration of the springs as shown schematically in Figure 3.7 when changing the 
relative configuration of i and b. This last subsystem represents the IPC of Figure 3.8. 
In the figure, bond graphs notation is used: each power port of Eq. (3.23) corresponds 
to a power bond in the figure. 

In order to use the IPC for the real control of a robotic system, we need a way to map 
w° to the actuation of the robot. This is done by using the robot differential kinematics, 
expressed by the Jacobian matrix J r (q), that maps a configuration velocity q to the 
twists t° of the tips of the hand: 



t° T = Jr(q)q 
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which implies 




J?(qW 



(3.24) 



V 



where W° is the total wrench applied to tip i by the controller's spring i. From the 
above equations, it could be deduced at first that, for the given GPCHS representation 
of the controller, the velocities of the joints are needed for the proposed scheme. This 
is not the case, since the controller's actions (i.e. the torques) applied to the robot are 
a consequence of the springs attached to the fingertips, as shown in Figure 3.6. These 
elastic forces are function only of the relative configuration of the tips with respect to 
the virtual object. In particular, this implies that only the pose of the tips are necessary 
to calculate these wrenches, and not their velocities. The configurations can be directly 
computed by using joints measurements and the forward kinematics. 

Clearly, this has the advantage that only position measurements are necessary to im- 
plement the scheme: no velocity and not even force measurements are in fact required. 
On the other hand, note that this scheme can therefore be used if the robot is back- 
drivable, which means in particular that any external force applied to the robot will 
result in a motion of the robot itself, that can be therefore detected with position mea- 
surements alone. 

This scheme does not change the driving point mass of the robot, but only its inter- 
active compliance, differently than the well known impedance scheme presented in 
(Hogan 1985b). It should be noticed though that even if different from (Hogan 1985a, 
Hogan 1985b, Hogan 1985c), the adopted philosophy is exactly the same as it has been 
commented in (Won, Stramigioli & Hogan 1997). 



Energy dissipation 

Even if only positions measurements are needed, overall dissipation is achieved be- 
cause of the presence of the viscous term connected to the virtual object. The viscous 
force is applied to the virtual object as shown in Figure 3.6. Any energy entering the 
control system which causes a motion of the virtual object is dissipated, and this has a 
nice physical interpretation. Again, note that this is achieved without the necessity to 
measure velocities of the robotic system. 

A control design philosophy very similar to this one has been developed by Ortega and 
others, see for example (Ortega, Loria, Kelly & Praly 1994). However, it has to be pointed 
out that in this case the formulation is based on a Lagrangian framework. Here, the 
problem is considered in the more general Hamiltonian setting as the power consistent 
interconnection of GPCHSs. 



A novel architecture has been presented which slightly resembles the human physi- 
ology. Real-time behavior is controlled by the Intrinsically Passive Controller which 
corresponds to a virtual physical system. The IPC more or less resembles the role of 



3.4 Summary 



3.4. SUMMARY 
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muscle and spindles in biological systems. The IPC together with the robot can be seen 
as a pre-compensated robot. Due to the implementation of the IPC control as a power 
consistent interconnection with the robot to be controlled, passivity is ensured in any 
situation if no power is injected by the Supervisor. The structure of the IPC is that of a 
Port Controlled Hamiltonian System with dissipation. An example of such an IPC has 
been given. An important point is that the IPC can be really designed using mechanical 
analogies like springs, dampers and masses and then implemented using the theory 
of interconnection of Port Controlled Hamiltonian Systems. Furthermore, it has been 
shown in (Stramigioli et al. 1998) using the theory of Casimir functions that it is pos- 
sible to implement the IPC with only measurements of positions q and not of velocities. 
With the presented strategy, it is not appropriate to talk about position or force control 
anymore, because what it is controlled is actually the behavior of the system and not 
the position or force at its interconnection port. This has the advantages of being very 
robust with respect to different materials and object with which the robot interact; just 
think about shaking hand to somebody, we never have a perfect model of the person 
we shake hand with, but the interaction is always well behaved. 



INTERACTION AND CONTROL 



Projective geometry and 
kinematics 



An extention to the conception we have of the three dimensional Eucledian spaces can 
be found in projective geometry 1 . To talk about the 3D Eucledian world in a projective 
setting, we need three ingredients: 

• A real vector space V 4 called the supporting vector space of dimension 4 from which 
we exclude the origin. 

• An equivalence relation on V 4 — {0}: V\ ~ i' 2 o 3a e 1 ^ s.t. v\ = av 2 - 

• A polarity P, which is a 2 covariant, symmetric tensor defined on V 4 which in the 
sequel will be taken semipositive defined and of rank 1 . 

The basic transformations between points of projective spaces are defined as injective 
linear transformations between the supporting vector spaces. These transformations 
must be injective to prevent that the subspace corresponding to the kernel of the trans- 
formation is mapped to the element of the codomain which is NOT a valid element 
of the projective space 2 . These kinds of transformations are called homographies or 
collineations and in our case are mappings from V 4 to V 4 . Maps from V 4 to the dual V 4 * 
are instead called correlations. A symmetric correlation as P is called polarity. 



Improper hyperplane 

Using the polarity P, it is possible to consider the vectors p L belonging to the quadric 
defined by the polarity P which is called the absolute: 

P ljP l P> = tfeV 4 . 

The absolute, is a three dimensional subspace of V 4 which is called the improper hy- 
perplane and it is indicated with 2 3 c V 4 . This hyperplane represents the "points at 
infinity". 

The improper hyperplane splits V 4 in two disjointed semi-spaces which we will call 
respectively positive semi-space and indicate it with X + and negative semi-space and 
indicate it with T~ . 

l C&\ey in 1859, by introducing the concept of an "absolute", showed that projective geometry is the most 
general. 

2 It will be shown later that to have a proper definition of the projective space, the element must be 
excluded. 
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A. PROJECTIVE GEOMETRY AND KINEMATICS 



The three dimensional projective space is defined as the quotient space of V 4 excluding 
the origin with respect to the defined equivalent relation: 

p3 . V 4 - {0} 



In a purely projective setting, without considering the polarity, all points are of the 
same type. Considering the polarity, we can make a distinction between finite points 
and infinite points. Infinite points are those whose representative in V 4 belong to I 3 
and finite points are the others. We indicate with Vf the finite points and with Voo the 
points at infinity. Clearly we have that V = Vf U Voq. Furthermore, for each finite point 
p <eVf {PijP % P 3 7^ 0), there are representatives v e V 4 belonging either to 1 + or to T~ . We 
can define the sign function a for elements v e V 4 : 

r +i v e 1+ 

<r(v) := < v el 3 (A.1) 

[-1 veJ- 



Adjoint polarity 



Associated with the polarity P, one defines its adjoint 3 Q which is a 2 contravariant, 
symmetric, semipositive tensor of rank 3. Once a proper base {e x , e y , e z , eo} for V 4 is 
chosen, the representations of P and Q become: 



P= n n n n (A-2) 
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In the same coordinates, a vector of V 4 has the form (x, y, z, a) T and if a = the vector 
belongs to I 3 . 



Points and Free vectors 



It is possible to associate to each pair of finite vectors in Vf a unique element of I: 

p q 



f:V F xV F ^l; (p,q)» 



Wip) IMIcK 1 ?) 



(A.3) 



where p, q are any representatives and | ■ | represents the P-norm. It is possible to see 
that the previous operation is indeed independent of the representatives of the points 
and therefore well defined. Note that the difference of two points can be calculated 
using the vector structure of V 4 . Furthermore, the improper hyperplane without the 
equivalence relation and with the origin of V 4 , gets the meaning of the vector space of 
free-vectors. In the usual coordinates, this means that if 







( a q x q\ 




and q = 


a q y q 


(XpZp 




OLqZq 


\ a v ) 




\Oiq J 



3 If instead of on the diagonal elements of P and Q we substitute e, we see that QP = PQ = el where I is 
the identity matrix. 
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we have | |p| jcr(p) = a p and ||g||cr(g) = a q and therefore 



A 



Vp ~ Vq 
Zp — Zq 

V o ) 



Note that indeed the last component is equal to zero which confirms the fact that the 
element belongs to X. It is usual to use the notation: 

p-q~ f(p, q) 

for obvious reasons, but the normalisation of Eq. (A.3) before the subtraction is essen- 
tial to make the operation intrinsically denned. 



Lines 



A line I c V 3 in a projective context is nothing else than a one dimensional subspace 
which corresponds to a two dimensional subspace L c V 4 (without the origin) of the 
supporting vector space V 4 . In a projective context it is also possible to talk about lines 
at infinity when L c I 3 . As a consequence, a line can be described both as the subspace 
spanned by two points of V 4 or as the intersection of two hyperplanes of V 4 . As in every 
2n dimensional vector space an n dimensional subspace is self-dual, so is a line in V 3 a 
self dual entity. 

It is possible to show that using the previous coordinates, given two distinct points x, y 
belonging to a line where 

x = [ X ] and I V ] where x, y E R 3 



1 ) \1 / 

the corresponding line can be identified homogeneously to a vector of the form 

a f | ~ ^) where ael - {0}. (A.4) 
Eq. (A.4) can be clearly written as: 



y-x \ = I u 
x A (y — x) J \f A u 



(A. 5) 



It is easy to see that choosing instead of x and y other two distinct points on the same 
line in the usual geometrical sence, the vector I denned as in Eq. (A.4) is the same 
if a proportional multiplication constant is used. This implies that the space of lines 
has somehow a projective nature by itself, but it should be noticed that the linear 
combination of two lines as expressed in Eq. (A.4) is in general NOT a line. 



Euclidean product on the improper hyperplane 

It is now possible to define a proper non-singular, internal product for I 3 which gives 
rise to the scalar product which characterises a proper Euclidean Space. In the previous 
canonical coordinates, an element v t g I 3 is characterised by the last component equal 
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to zero. We can associate to «j e I 3 the subspace of hyperplanes $j Vi for which Vi is what 
is called the polar with respect to Q: 

h G Vi = Qh 

where Q is the adjoint of P. We can now define the scalar product of two vectors wi,w 2 
in J 3 as: 



It is easy to see that the previous definition is well posed since it is independent on the 
elements hi and h 2 due to the structure of Q. 



Introduction to Lie groups 



A manifold is intuitively a smooth space which is locally homeomorphic to M" and brings 
with itself nice differentiability properties. Proper definitions of manifolds can be found 
on (Dubrovin, Fomenko & Novikov 1985, Boothby 1975). A group is an algebraical 
structure defined on a set. Definitions of groups can be found on any basic book of 
algebra. 

A Lie group is a group, whose set on which the operation are defined is a manifold Q. 
This manifold Q has therefore a special point 'e' which is the identity of the group. 

Using the structure of the group, and by denoting the group operation as: 

o : Q x Q — > g ; ; (h,g) i-> h o g, 

we can define two mappings within the group which are called respectively left and right 
mapping: 

L g : g — > g ; h h-> g o h (B. 1) 

and 

Rg:Q->Gl h^hog (B.2) 

As we will see later the differential of these mappings at the identity, plays an important 
role in the study of mechanics. 

The tangent space T e g to g at e, which is indicated with g, has furthermore the structure 
of a Lie algebra which is nothing else than a vector space g together with an internal, 
skew- symmetric operation called the commutator: 

[,]: x ; 92) i-> [51,32] (B.3) 

For g to be a Lie algebra, the commutator should furthermore satisfy what is called the 
Jacoby identity: 

[91, [92, gs]) + [g2, [33, 3i]] + [33, [31,32]] =0 V3i,32,33 Gfl (B.4) 

Lie groups are important because we can use them as acting on a manifold M , which 
in our case will be the Euclidean space. An action of Q on M, is a smooth application 
of the following form: 

a : g x M M 

such that 

a(e, x) = x \fx € A4, 

and 

a(g 1 ,a(g 2 ,x)) = a(gig 2 ,x) \fx e M,g\,g 2 G Q. 
This means that an action is somehow compatible with the group on which it is defined. 
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B.l Matrix Lie groups 

For a lot of fundamental reasons like Ado's theorem (Olver 1993), matrix algebras are 
excellent representatives for any finite dimensional group like the ones we need for rigid 
body mechanisms. 

A matrix Lie group is a group whose elements are square matrices and in which the 
composition operation of the group corresponds to the matrix product. The most gen- 
eral real matrix group is GL{n) which represents the group of non singular n x n real 
matrices. This is clearly a group since the identity matrix represents the identity el- 
ement of the group, for each matrix, there is an inverse, and matrix multiplication is 
associative. We will now analyse more in detail features and operations of matrix Lie 
groups. 



Left and Right maps 

If we consider a matrix Lie group Q, the operations of left and right translation clearly 
become: 

L G (H) = GH and R G (H) = HG. 

We can now consider how velocities are mapped using the previous maps. Suppose that 
we want to map a velocity vector H e TjjQ to a velocity vector in TqhQ using the left 
translation and to a vector in T H gQ using right translation. We obtain: 

[L G ).,{H, H) = {GH, GH) and {R G )*{H, H) = (HG, HG) 

In particular, if we take a reference velocity at the identity, we obtain: 

(L G )*(I,T) = (G,GT) and (Rg)*(I,T) = (G,TG) 

where Teg. With an abuse of notation, we will often indicate: 

(L G )» T = GT and (i? G ), T = TG 

when it is clear that we consider mappings from the identity of the group. On a Lie 
group, we can define left invariant or right invariant vector fields. These vector fields 
are such that the differential of the left invariant and right invariant map leaves them 
invariant. If we indicate with 

V : G ^ TQ ; x i-> (a;, v) 

a smooth vector field on the Lie group Q, we say that this vector field is left invariant if: 

V(L g (h)) = (L g ),V(h) Vg,heG, 
and similarly it is right invariant if: 

V(R g (h)) = (R g )*V(h) VgeG. 
For a matrix group, if we take in the previous definitions h = I we obtain respectively: 

V(G) = GT L and V(G) = T R G 

where we indicated the representative of the left and right invariant vector fields at the 
identity with T L and T R . We can conclude from this that any left or right invariant vector 
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field is characterized completely by its value at the identity of the group. We could now 
ask ourself: what are the integrals of a left or invariant vector field? From what just 
said, the integral of a left invariant vector field, can be calculated as the integral of the 
following matrix differential equation: 

G = GT L => G(t) = G(0)e TLt (B.5) 

where T L is the value of the vector field at the identity. In a similar way, the integral of 
a right invariant vector field is: 

G = TrG =>• G(t) = e TRt G(0). (B.6) 

From this it is possible to conclude that if we take an element T e g, its left and right 
integral curves passing through the identity coincide and they represent the exponential 
map from the Lie algebra to the Lie group: 

e:g^G;T^e T . 

It is easy to show, and important to notice, that integral curves passing through points 
H = e Tl of right and left invariant vector fields which have as representative in the 
identity T 2 , are coincident iff e Tl e T2 = e Ta e Tl which is true iff [Ti, T 2 ] = 0, where the last 
operation is the commutator of the Lie algebra. But how does the commutator look 
like for a matrix Lie algebra ? Being a Lie group a manifold, we can compute the Lie 
brackets of vector fields on the manifold. Furthermore, we know that elements of the 
Lie algebra g have a left and right vector field associated to them. We can than calculate 
the Lie bracket of two left or right invariant vector fields, and if the solution is still left 
or right invariant, consider the value of the resulting vector field at the identity as the 
solution of the commutator. We will start with the left invariant case first. Consider we 
are in a point G(t) e Q at time t. If we have two left invariant vector fields characterized 
by Ti,T 2 G g, the Lie bracket of these two vector fields, can be calculated by moving 
from G(t) along the vector field correspondent to Ti for s/s time, than along the one 
correspondent to T 2 , than along -Ti and eventually along —T 2 . In mathematical terms 
we have: 

G(t + V^) = G(t)e Tl ^ -» G(t + 2y/a) = G(t + ^~s)e T ^ s -» 

G(t + 3v^) = G(t + 2^) e - Tl ^ -» G(t + 4v/i) = G(t + 3v^)e~ T2 ^ -> 

G[t + Vi) = G(t)e Tl ^e T ^e- T ^e- T2 ^ (B.7) 

If we look at j^G(t + 4^/s)\ s _ Q , we can approximate the exponentials with the first low 
order terms and we obtain: 



G{t + A^~s) ~ G(t) 



- 2 



2 ~ I V ~* y " ' 2 S 



G(t) (7 + (T 1 T 2 -T a T 1 )a + o{a)) (B.8) 



which implies 



±G(t + A^) 
as 



G(t)(TiT 2 — T 2 Ti). 



From the previous equation, we can conclude that the resulting vector field is still left 
invariant and it is characterized by the Lie algebra element TiT 2 — T 2 T t . We can therefore 
define the commutator based on left invariant vector fields as: 



[T 1 ,T 2 ] L =T 1 T 2 -T 2 T 1 . 
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With similar reasoning, it is possible to show for right invariant vector fields that: 

d 



ds 



and therefore, in this case: 



We have therefore that: 



G(t + 4v^) 



(T 2 Ti — TiT 2 )G(t). 



[T 1 ,T 2 ] R = T 2 T 1 -T 1 T 2 . 



[Ti,T 2 ] l = -[T lt T 2 ] R . 

In the literature, [,] L is used as the standard commutator and we will adapt this con- 
vention. 



B.l.l Matrix Group Actions 



A group action we can consider for an n dimensional matrix Lie group is the linear 
operation on M™. We can therefore define as an action: 

a(G, P) = GP GeG,Pe K" 

It is easy to see that this group action trivially satisfies all the properties required. 



B.1.2 Adjoint representation 

Using the left and right maps, we can define what is called the conjugation map as 
Kg := R g -iLg which for matrix groups results: 

K G - g^Q- H^GHG" 1 . 

But what is the importance of this conjugation map ? To answer this question, we need 
the matrix group action. Suppose we have a certain element H g g such that Q = HP 
where Q,P eK' 1 . What happens if we move all the points of R™ and therefore also Q and 
P using an element of g ? What will the corresponding mapping of H look like ? If we 
have Q! = GQ and P' = GP, it is straight forward to see that: 

Q' = K G {H)P'. 

The conjugation map is therefore related to global motions or equivalently changes of 
coordinates. We clearly have that Kq{I) = I and therefore the differential of Kg() at 
the identity is a Lie algebra endomorphism. This linear map is called the Adjoint group 
representation: 

Ad G — 0; T^GTG- 1 . 

The Adjoint representation of the group shows how an infinitesimal motion changes 
moving the references of a finite amount G. Eventually, it is possible to consider the 
derivative of the previous map at the identity 



adx '■= —Ad e T 
ds 
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This map is called the adjoint representation of the Lie algebra and it is a map of the 
form: 

ad T : q -> g Teg 
If we use the definitions we can see that: 



— Ad eTl sT 2 
as 



s=0 



as 



= TiT 2 -T 2 T 1 = [Ti,T 2 ] L 



which shows that: 



ad Tl T 2 = [Ti,T 2 ]l 



(B.9) 
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